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ABSTRACT 


United States Department of Defense (DoD) autonomous vehiele efforts have 
eoneentrated researeh in areas that support development of unmanned ground and air 
battlefield vehieles. Little attention has been paid to applying roboties to automate 
routine tasks. A robotie solution eonsisting of a prototype holonomie vehiele is proposed 
to seareh for, deteet, and remove debris that eould eause foreign objeet damage (FOD) to 
turbine-engine aireraft operated from ships. Holonomie, or omnidireetional, motion was 
realized by solving the system of equations governing the vehiele’s motion atop a plane 
surfaee. Translational motion without chassis rotation was achieved through motion 
control using a single board computer, a pulse width modulation (PWM) and optical 
isolation circuit, and a low-cost inertial measurement unit (IMU). Obstacle detection and 
avoidance was realized by constructing a microprocessor-controlled scanning ultrasonic 
sonar detector head and controller circuit. The sonar detector demonstrated 360° 
coverage and centimeter resolution. Rudimentary autonomous operation and wireless 
manual control via a Java graphical user interface (GUI) were achieved in an indoor 
environment. 
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I. 


INTRODUCTION 


In both civilian industry and the Department of Defense (DoD) robotics continue 
to grow in importanee as the eapabilities of eomputer proeessors inerease and researehers 
pursue ways to exploit these gains. Presently, roboties eomprises a diverse field ranging 
from stationary industrial maehines to vehieles able to traverse planets beyond the reaeh 
of any human being to date. Like the National Aviation Space Administration (NASA), 
the DoD has reeognized the utility of autonomous vehieles to complete missions ill-suited 
to humans due to the mission’s hazardous nature, the hostility or remoteness of the 
environment, its tedious nature, its long duration, or other faetors. 

A. HISTORIC MILITARY ROBOTIC TASKS AND RESEARCH 

Historieally, the DoD’s efforts in unmanned autonomous ground vehieles have 
primarily been ehanneled into robotie devices, such as iRobof s PackBot, to conduct 
explosive ordnanee disposal (EOD) or to provide troops with reeonnaissanee in urban 
areas. Figure 1 shows the PaekBot in aetion. 



Figure 1. iRobof s PaekBot eondueting EOD task in Iraq. (From [1]) 
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The obvious desire to spare humans from the need to engage in potentially deadly 
work sueh as EOD represents one thrust in DoD robotics. Airborne reconnaissance and 
weapons employment by unmanned aerial vehicles (UAVs) such as the MQ-1 Predator 
and its successor, the MQ-9 Reaper, are missions whose characteristics, chiefly long 
duration and danger to humans, are clearly suited to robotics. Figure 2 shows the U.S. 
Air Force’s newly-operational MQ-9 Reaper. According to General Mosley, the Air 
Force chief of staff. Reaper’s 14 hour endurance is its primary advantage over human- 
piloted aircraft, not the fact that it removes humans from the battlespace [2]. 



Figure 2. MQ-9 Reaper FIAV. (From [3]) 


Within the DoD little effort has been devoted to applying robotic solutions to 
tedious tasks. Rather, DoD and the Defense Advanced Research Projects Agency 
(DARPA), through its DARPA Grand Challenge and Urban Challenge, have emphasized 
research into technologies to replace human-operated ground combat vehicles on the 
battlefield, in support of the Congressional mandate put into law with the National 
Defense Authorization Act for Fiscal Year 2001 [4]. Figure 3 shows the top three 
finishers from the 2007 Urban Challenge, a competition designed to promote research 
into robotic technologies necessary to remove humans from battlefield vehicles. 
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Figure 3. DARPA Urban Challenge Top Three Finishers. (From [5]) 


While sueh efforts are laudable, they negleet the utility of autonomous vehicles 
for menial, boring, long-duration, routine tasks. Every day men and women in uniform 
are called upon to complete necessary, albeit time consuming, tasks that waste valuable 
productive time. 

B. PROJECT MOTIVATION 

One such task is the ongoing safety and aircraft maintenance requirement to 
remove debris, or foreign objects, from areas such as flight lines, hangars, hangar bays, 
and flight decks of U.S. Navy installations and ships. Foreign objects are an amazingly 
broad class of debris that may include, but is not limited to, the following types of 
objects: 

• aircraft fasteners (e.g. rivets, screws, nuts) 

• safety wire 

• coins 

• tools 

• soda cans 

• flight gear 

• packaging materials (e.g. boxes, wooden splinters from crates) 
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When a foreign objeet is ingested by a turbine engine, regardless of whether it is a 
turbofan, turboshaft, or turboprop engine, the resultant damage, referred to as Foreign 
Object Damage (FOD) can be costly. If one considers simply the engine replacement 
cost data available from the Naval Safety Center, then the monetary costs associated with 
damage to a single F/A-18 E/F Super Hornet jet engine could reach $4.7 million [6]. 
Engine replacement costs, though, ignore the deadlier potential of EOD. EOD kills. Eoss 
of an engine during a critical phase of flight, such as landing or take off, could result in 
the complete loss of the aircraft, its aircrew, and the associated loss of the time and 
money invested in their training. The Naval Safety Center estimates the Navy and 
Marine Corp spend $90 million annually on EOD-related damage and devote tens of 
thousands of man-hours to preventing and repairing damage from EOD [7]. 

Currently, Navy prevention largely consists of “EOD walkdowns,” shown in 
Eigure 4, that require anywhere from 30 to 80 people. The EOD walkdown shown in 
Eigure 4 might require 100 people and take 20 minutes; that represents 30 man hours of 
work. Personnel walk side-by-side and visually scan the deck in front of them to detect 
EOD and then manually remove it. This task is competed several times each day and is 
required prior to conducting flight operations. The Navy’s current prevention method 
will become harder to accommodate in the future as the service fields newer ships 
designed to operate with reduced manning compared with ships presently in service. 

C. PROBLEM SOLUTION 

An autonomous solution to EOD detection and removal aboard ships could 
supplement or replace the present manual methods and allow those people to spend their 
time in more productive ways. Eurther, a robotic solution would never tire or become 
distracted, as can occur when humans conduct manual EOD detection and removal. 
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Figure 4. Typical FOD Walkdown aboard USS CARL VINSON (CVN 70). (From [8]) 
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II. HOLONOMIC MOTION 


A. MOTIVATION 

The autonomous vehicle conceived to address the problem of automated FOD 
detection and removal was dubbed the Creature. Its operating environment was 
anticipated to include numerous, closely-spaced obstacles. Previous SMART program 
robots have included conventional four-wheel, skid steered and tracked chassis designs, 
but these were intended for use outdoors in environments with greater obstacle spacing 
than the Creature’s proposed environment [9,10,11]. To achieve mobility in an 
environment such as a typical office, it was decided that the design should be capable of 
rotation in place within its own footprint. Kitagawa, Kobayashi, Beppu, and Terashima, 
describe the usefulness of a holonomic vehicle in exactly this sort of environment, and 
note that motion planning for such vehicles is simpler [12]. Robotic researchers have 
regularly employed holonomic designs in applications where extreme mobility is 
required, e.g., robot soccer [13]. The researcher decided that the FOD finding robot 
project offered a good avenue to explore holonomic motion. 

B. HOLONOMIC MOTION DEFINITION 

In robotics, a holonomic robot possesses an equal or greater number of 
controllable degrees of freedom (DOF) as the total DOFs of the system [14]. Past 
literature has also referred to holonomic vehicles as multi-directional or omnidirectional 
vehicles. Dickerson distinguishes between singular and non-singular multi-directional 
vehicles. A vehicle with independently steered wheels might be capable of multi¬ 
directional motion, but even “a small motion in some directions may require a large 
motion of the propulsion system,” which makes the vehicle singular [15]. In contrast, a 
non-singular vehicle such as the Creature, “can move a small amount in any direction 
without a large motion of the wheels” [15]. A holonomic vehicle operating on a plane 
surface is capable of instantly achieving translational motion in any direction while 
yawing. 
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C. DERIVATION OF EQUATION OF MOTION 

Kalmar-Nagy, Ganguly, and D’Andrea provide a derivation of the equation of 
motion for a three-wheeled omnidirectional vehicle as a prelude to their discussion of a 
less computationally demanding control theory for such a vehicle [16]. Rojas and Forster 
also provide a concise derivation [17]. We begin by considering an arbitrary Cartesian 
coordinate frame of reference about the center of mass of the robot. Additionally, assume 
the center of mass coincides with the geometric center of the robot’s circular chassis and 
is located at the origin. O', as in Figure 5. The positions of the robot’s omniwheels about 
its center of mass are functions of the angle (p. 



The vector r'i, describes the position of the i-th wheel. For a counterclockwise 
rotation angle, (p, from the X' axis, a rotation matrix R((p) given by Equation 2.1 can be 
applied to each wheel position vector to obtain the wheel position in the Cartesian frame 
of the robot (X' Y')^. 


R{(p) = 


coscp 

smcp 


-sin^ 

cos(p 


( 2 . 1 ) 


8 



For the first wheel the angle (p is arbitrarily set equal to nil, thus aligning r'l 


with the Y' axis. If the wheels are arranged 120° apart and loeated at distanee B from O' 
then their positions in the robot frame are: 


n 


r\=R\ - B 


In 

3 


= B 


^n 

3 



"O' 


r'l =R —\b 


= B 

J ' 1 3 ^ 

1 


3 . ^ An\ 

"O' 

~ 

II 

Co 


= B 

2 ' V 3 2 

1 



-V3/2 

- 1/2 

V3/2 

- 1/2 


The drive direetion of eaeh wheel is perpendieular to the wheel position veetor. 
The unit veetor deseribing the drive direetion, D'i , of the i-th wheel is obtained by 
applying the rotation matrix R( ;r / 2 ) to the position veetor of the wheel. For the number 
one wheel the drive direetion is given by Equation 2.2. Equations 2.3 and 2.4 give the 
drive direetions for the number two and three wheels, respeetively. 


^ n^ 

. 1 ^1 

{ n^ 


“O' 




r 1 = —R\ 
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= 


Jj 

‘ B 1 

l2j 
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( 2 . 2 ) 


f-V 

— r, =—R\ 

\—]b 

“O' 


■ 1/2 ■ 

UJ 

L 3 j ' B 1 

1 6 j 
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_-V3/2_ 


f-V 

/ 4^1 . 1 D 

— r , = —R 

Old'll; 

“0" 


■ 1/2 ■ 

UJ 

1 3 j ' 5 

1 6 j 

1 


_V3/2_ 


(2.3) 

(2.4) 


The position of the robot’s eenter of mass at O' is related to the Newtonian Earth 
frame of referenee by the veetor, Rq. Additionally, the two frames may be rotated by an 
angle 0 with respeet to eaeh other, so a rotation matrix, R(0), ean be applied to relate any 
position veetor in the robot’s frame to the Earth Erame, (X Y)^. 
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Figure 6 is a simplified view showing the relationship between the positions of 
the number one wheel in eaeh frame. Let the veetor ri be the position of the i-th wheel in 
the Earth frame. Then the position of the wheel, ri, is the sum of the veetor Rq from the 
Earth frame origin, O, to the center of mass plus the vector r'i rotated to account for the 
rotation between the frames. 



Eigure 6. Wheel Position in Earth and Robot Reference Erames. 

Equation 2.5 gives the position vector in the Earth frame as a function of the 
vector from the origin, O, to O' and the rotated vector from the robot frame. 


ri=R^+R(^)r,' 


r,=R^+Ri0X' = R^+Ri0)B ^ 

= Ro + RiOX ' = Ro+ R{0)R{2nl3X ' = Rq + 
r^ = Ro+ Ri0)r ,' = R^ + R{0)R{Anly)r ,' = R^ + R{0)B 


-Sl2 
. -1/2 

Sl2 

-l/2_ 


(2.5) 
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The velocity at the wheel location, Vi, is obtained in the same manner by taking 
the time derivative in Equation 2.6. Since the vectors r'i are constant with time in the 
robot’s frame, their time derivatives are zero, and the velocities in the Earth frame are 
given by: 


cIKq ^ dR{6)r.' 
dt dt 


• dR{6) , 

Ro + — ^^rd+R{0) 


dt 


dt 


V, =Ro+7?(^)r.' 


Vj =Ro + i?(^)rj' 

Vj = Ro+' = Ro + rIo) 7?(2;z-/3)rj' 
V 3 = Ro + i?(^)r 3 ' = Ro + i?(^)7?(4;r/3)rj ’ 


( 2 . 6 ) 


At each wheel, i, the wheel velocity, v \ , must be aligned with the unit vector D'i 
that defines the drive direction for that wheel. Alternatively, one can think of the wheel 
velocity arising from the dot product of the velocity at the drive point and the drive 
direction, since the wheel’s axis is fixed and perpendicular to the drive direction. 


v2 = V, • R{0)Yi ^' or V,' = 






One can substitute the velocity Vi above into the equation, to derive Equation 2.7. 




- 


IX 

R(m'= 




Ro+i?(^)r,' 




. T f . \T 

v,.' = Ro R{0)Dd+ R{e)r.' 7?(^)D7 

V y 

. T / Y . T 

v,.' = Ro R(0)D/+ r,' R(0) R(0)D.' 

V y 


v,.' = Ro R(0)D/+BQ 


(2.7) 
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In Equation 2.7 the tangential velocity, was substituted for the second term in 
the last step. One can see the wheel velocity contributes to a linear velocity component 
that produces translational motion, and a rotational velocity component responsible for 
rotation about the center of mass. If each wheel has radius b and rotates at an angular 
speed, CO, the wheel velocities, v,., can be written as in Equation 2.8. 

~ 0)^1 -COS0 -sin^ B ^ 

b a >2 = 1 / 2 cos6* + Vs/2sin ^ l/2sin^-V3/2cos6’ B y (2.8) 
co^j _l/2cos6’-V3/2sin6’ l/2sin6’ +V3/2 cos 6* -Sj (9 

If each wheel’s action against the surface produces a force, fi, due to friction 
along the D'i direction, then the total force on the robot is Fr, which is obtained by 
summing the three individual drive forces. 

i=l 

In the Earth frame this can be expressed using the rotation matrix. 

F.=yf,S(«)D,' 

(=1 

Conservation of linear momentum gives the equation of motion of the robot, 
which uses the rotation matrix above to relate forces in the robot frame to the Newtonian 
Earth frame in Equation 2.9. 

=Ma 

= (2.9) 

,=i dt 
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Conservation of angular momentum requires the time rate of ehange of angular 


momentum, L, equal the torque , F, applied. 


clL 

dt 


= r 


Considering only the forces acting parallel to the x-y plane at distance B and the 
angular momentum, L, about the robot’s Z' axis, one can write the conservation equation. 
Then one can replace the time rate of change of the angular momentum with the moment 
of inertia, I, and the acceleration of the robot’s chassis rotation, which is equivalent to the 
second derivative with respect to time of the angular displacement between the two 
coordinate frames as shown in Equation 2.10. 


dU 

dt 




I 


d^e 

dt^ 




( 2 . 10 ) 
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III. EXPERIMENTAL DESIGN 


The Creature is a three-wheeled omnidireetional, or holonomic, robot eapable of 
semi-autonomous operation. It was eonceived as a prototype to address the problem of 
automated FOD detection and removal as well as to provide a research platform for 
future SMART Program efforts. Its major components are shown in Figures 7 and 8. 
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Three Wheel 
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5V DC/DC Switching 
Regulator 
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Wheel 2 


Figure 7. 


Diagram of Major Components Installed on Lower Level. 
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Wheel 2 

SO' 

Diagram of Major Components Installed on Upper Level. 


A. MECHANICAL CONSTRUCTION 
I. Chassis 

The ehassis design required a platform that was light-weight, strong, and large 
enough to provide area for mounting components required for the robot’s operation, such 
as batteries, motors, sensors, CPU, etc. A SuperDroid Robots three-wheeled vectoring 
robot kit consisting of motors, wiring, and chassis was purchased with the intent to use it 
as the robot’s chassis and propulsion. The vendor-supplied equilateral triangular base, 
measuring eleven inches on a side, was deemed too small because it would not have 
allowed sufficient space to mount the anticipated equipment or allowed for growth. A 
larger, low-cost chassis was constructed by reusing a 1/8-inch thick aluminum plate 
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recovered from a computer’s backplane. A seven-inch radius disk with a mass of 838 g 
was cut from the plate. It was drilled to mount the three motor mounts to the underside 
of the plate at 120“ intervals. The disk also served as a ground plane to isolate the 
electronics, sensors, CPU, and devices powered by the electronics bus from radio 
frequency interference (RFI) produced by the three brushed direct current (DC) motors 
below the chassis. 

During construction of the chassis, provision was made to mount three 12 V, 
batteries on the underside of the chassis disk in the space between each motor mount. 
Since the batteries were among the most massive components installed, placing them at 
roughly the same height as the wheel axles was believed to offer improved stability 
compared to placing them atop the chassis. To increase the robot’s moment of inertia 
about its Z axis, the batteries were placed 11.5 cm from the chassis’ geometric center 
rather than directly under it. Based on the derivation of the equation of motion in 
Chapter II, one can see that a larger moment of inertia would help maintain directional 
control in cases where a torque was applied due to imbalances in motor speeds. 

The triangular aluminum plate supplied by the vendor was briefly considered for 
use as a second level for mounting the CPU and sensors. Figure 9 shows the chassis at an 
early phase of construction with the original triangular top plate. A redesign of the 
electrical power supplies, described in the Electrical System Design section, required 
replacing the triangular plate with a larger mounting surface. A Plexiglas square, 
supported by four-inch-long aluminum standoffs was installed in its place. The square 
plate provided a larger mounting surface. Sonar sensors were mounted atop it, and the 
large, flat electronics battery and router are hung beneath it. Four screws attach the top 
plate to the standoffs, and all devices attached to the top plate feature electrical quick 
disconnects to allow the operator to remove the top plate in under two minutes for easy 
access to components mounted atop the chassis. 
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Figure 9. Bare Chassis with Motors Installed. 


Figure 10 shows the Creature in a later stage of development. In this photo the 
first generation eleetronies bus, visible on the left, is still installed, and the buek 
switehing DC/DC eonverter has not been installed yet. 



Figure 10. Creature Later Development Configuration. 
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Omniwheels 


The heart of the Creature’s holonomic mobility is its three Kornylak Transwheels. 
The plastic wheels have a 4 inch O.D. and two rows of eight free-turning rollers. The 
wheels’ measured diameter was approximately 10 cm, which agrees with the 
manufacturers’ specification. The rollers are oriented 90“ to the direction of the motor’s 
axle. The Creature uses the double row model 4000 series, which are rated at 100 pounds 
per wheel [18]. The Creature’s weight during the experimental observations was 20 lbs; 
thus the load supported by any individual wheel is well within limits. 

B. PROPULSION AND CONTROL 

1. Motors and Motor Controllers 

The Creature is propelled by three 12/24V IG32 032 mm brushed DC gear 
motors. The motors have a mass of 194 g and a rated speed of 195 rpm. According to 
the manufacturer’s specification sheet, the planetary gear’s 1:27 reduction ratio applied to 
the 6500 rpm no-load motor speed produces an output wheel speed of 240 rpm or 8n 
radians/s. Rated torque is 1.4 kg cm. Operated at 24 V, the motors draw a maximum of 
1.1 A under 350 g cm load [19]. The vendor’s motor wiring kit, which includes ferrite 
rings and shielded hook-up cables, was used to connect the output terminals of the motor 
controllers to the DC motors’ leads. The cables’ internal foil shielding is grounded to the 
chassis to reduce or eliminate unwanted radiated electromagnetic radiation from the DC 
motor leads, which could act as transmitting antennas. 

The motors were installed under the chassis inside the box-like motor mounts 
supplied with the vendor’s vectoring robot kit. The mounts are constructed of welded, 
1/8-inch aluminum, and each mount’s mass was 205 g. In addition to using the chassis as 
a ground plane to isolate the CPU and electronics from RFI from the DC motors, each 
motor was wrapped in a metal screen, forming a small cylindrical Faraday cage around it. 
Figure 11 shows the underside of the robot. One motor is visible inside its Faraday cage 
in the photograph. A motor controller is visible on the left. Reducing the length of the 
motor leads was another RFI reduction measure. 
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Each motor receives DC eleetrical power independently via a SuperDroid Robots 
3A PWM motor controller. This controller uses an LMD18200 H-bridge motor driver, 
which is rated at 3 A [20]. The LMD 18200 is operated in the sign/magnitude mode, i.e., 
the controller is supplied two signals, the digital PWM magnitude and the desired motor 
direction. A lack of PWM signal is interpreted as zero magnitude. Using the 
sign/magnitude mode provided a finer control of the wheel speeds by allowing the 
robot’s CPU to map its entire analog output range to the desired wheel speed. Previous 
SMART program robots have operated the same PWM motor controller in a bidirectional 
magnitude mode, which decreased the resolution of the speed control and resulted in 
unsteady operation at slow speeds or when stopped [11]. The motor eontroller’s brake 
signal is not used, and it is held in a low logic state. 



Figure 11. Shielded DC Motor inside Motor Mount. 


2. PWM and Optoisolation Circuit 

One design goal for the Creature was to distribute the computational load and the 
CPU’s workload. Other autonomous vehicles constructed as part of NPS SMART 
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program have used the BL2000 single board eomputer, a 20 MHz proeessor that offers 
modest eomputational speed. It has proven adequate when tasked with running the robot 
operating program [11], It was deeided that the CPU would control motor speeds with an 
analog speed signal rather than devote CPU cycles to producing a pulse width modulated 
(PWM) signal. Three design challenges arose from this. First, the motor controllers 
described above require a PWM input, so the CPU’s analog signal required a circuit able 
to produce PWM signals from an analog voltage. Second, electrically isolating the CPU 
on a separate power bus from the motor controllers and DC motors required optical 
isolation, which in turn required that the CPU’s signal be converted to a digital form. 
Third, the digital direction signals for each motor also needed to be passed from the CPU 
to the motor controllers, requiring optoisolation for these signals as well. 

a. Early Efforts in Pulse Width Modulation of Motor Speed Signals 

Intersective pulse width modulation has been implemented previously 
using the analog speed signal as a reference and comparing it against a regularly varying 
signal. Miller provides an explanation of a PWM circuit that has been previously used on 
the Autonomous Ground Vehicle (AGV) SMART program robot. The circuit was based 
on a LM 555 timer IC [10]. A number of oscillator circuits were explored, including 
crystal oscillator and LRC tank circuits, in an attempt to improve on the existing PWM 
circuit. 


b. Crystal Oscillator Circuit 

A simple 100 kHz crystal oscillator tank circuit using an LM741 opamp 
was fed to a LM393N comparator. The circuit’s schematic is provided below in Figure 
12. A significant complication with the crystal oscillator circuit was that it and other 
sinusoidal oscillator circuits required positive and negative rail voltages. Providing 
positive and negative rail voltages would have complicated the electronics power supply, 
so single supply solutions were preferred. Negative triangle wave voltages were not 
desired, so a diode clamp was used to raise them to a minimum of -0.4 V. Later a 3 kD 
pull-up resistor, not shown, was connected to the PWM signal output to pull-up the 
output to positive levels. 
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The circuit showed promising results, and produced a very linear response. 
In prototype testing the triangle wave of the crystal oscillator and the analog signal were 
provided to the comparator, and the PWM signal’s duty cycle recorded. Then the 
comparator opamp was provided the reference analog signal and a 100 kHz triangle wave 
from a function generator. The crystal circuits’ data were virtually identical to that 
obtained with the function generator providing the triangle wave. A linear fit of the 
crystal circuit’s data was obtained; the circuit produced a 0% duty cycle at 0 V, and a 
100% duty cycle at 3.730 V. The analog speed signal range corresponded well with the 
analog output voltage limits of the BL2000, which was the target CPU at the time. 



Figure 12. Crystal Oscillator PWM Circuit. 

Ultimately, the crystal tank circuits’ frequency proved too high to be 
practical. The 2531 optoisolator IC available for use on the robot distorted the signal 
above 10 to 11 kHz due to insufficient slew rate. Further, it was discovered the motor 
controller manufacturer recommended an input signal frequency of 1 kHz. 
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c. Final PWM Circuit using RC Oscillator 

Inductor resistor capacitor (LRC) tank circuits were unsuccessfully tried 
and discarded. In the end resistor capacitor (RC) oscillator circuits were investigated 
because of their lower frequencies. A complimentary metal oxide semiconductor 
(CMOS) relaxation oscillator using LM741 opamps was tested. Its triangle wave output 
was not symmetric nor was the slope, despite trimming the opamp. Researeh provided a 
good example that formed the basis of the cireuit that was installed on the Creature [21]. 
It used one quad LM324 opamp in place of the two LM741 opamps. To better 
accommodate the single voltage supply of the robot and increase the triangle wave’s 
output voltage range, a LM6132 10 MHz, rail-to-rail opamp was substituted for the 
LM324. 

Figure 13 shows a portion of the complete circuit that was manufactured 
and installed. It shows one channel and omits the portion of the circuit responsible for 
optical isolation. The circuit proved compatible with the single supply electrical power 
available the on the robot’s electronics power bus owing to the use of the voltage divider 
reference voltage used as inverting input into opamp B. The LM6132 opamp B is 
eonneeted as a Schmitt trigger and acts as an astable multivibrator. The opamp A acts as 
an integrator on the first opamp’s output. It provides delayed negative feedback to the 
Schmitt trigger opamp. Because the Sehmitt trigger’s output is a constant equal to either 
the positive or negative rail voltage, integrating the constant voltage produces a signal 
whose voltage varies linearly with time. 
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Figure 13. Single Supply Triangle Wave PWM Cireuit. 


The frequency of the triangle output is a function of the resistor R15, 
capacitor Cl, and the Schmitt trigger’s thresholds. The circuit produced a PWM 
frequency equal to 1.33 kHz. The resultant triangle wave was supplied to a LM741 
opamp C configured as a comparator. 

The robot’s CPU, initially the BL2000, had an analog voltage output range 
0 to 4.096 V, so non-inverting amplifier was included in the circuit. Opamp D was 
configured as a non-inverting amplifier with a gain equal to two. The gain was selected 
so that an analog voltage equal to 4.096 V would coincide with the maximum voltage of 
the triangle wave and produce a 0% duty cycle PWM signal. Resistors R9 and RIO were 
chosen to be 18 kO and 23.1 kO, respectively. The opamp had the added benefit of 
providing high input impedance for the CPU’s signal. 

The PWM signal produced by the portion of the circuit above was passed 
to dual 2531 optoisolators next. Each device contains two emitter light emitting diodes 
(LEDs) to convert the electrical signal into an optical one and a detector to reverse the 
process and produce an electrical output voltage. Eigure 14 shows a portion of the circuit 
responsible for isolating one channel. A capacitor, C5 was installed for noise 
suppression. In this configuration, the 2531 was connected to produce negative pull-up. 
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Note, the PWM eireuit above produeed a 100% duty eyele when the analog speed signal 
was zero. The duty eyele deereased with inereasing analog voltage. The negative pull- 
up, or pull down, applied to the optoisolator inverted this, and the resulting duty eyele of 
the optoisolator’s output was 0% when an analog speed signal of zero was applied. It 
inereased linearly with inereasing voltage onee the voltage exeeeded the 1.2 V threshold. 
A unity gain amplifier was ineluded between the optoisolator and the eireuit’s output to 
impedanee mateh the motor eontrollers’ input. 



Figure 14. Optoisolation and unity gain amplifieation of PWM signal. 

The resistors R1 and R4 were ehosen to provide the eorreet forward 
eurrent. If, through the optoisolator. Use of the LM741 opamp as a eomparator ensured 
that the eireuit eould tolerate R1 resistanee of 550 without dragging down the PWM 
voltage due to impedanee mismatehing. Resistor R1 was ehosen sueh that If was equal to 
10 to 15 mA with PWM signal voltages 8 to 10 V peak to peak. Seleeting R4 equal to 
2.2 kn ensured suffieiently high output voltages. The optoisolated PWM voltages were 
observed to be 4.9 V peak to peak. 

The eireuit also passed three signals that eould be used for either brake or 
direetion signals to the motor eontrollers. Beeause of the negative pull up in the 
optoisolators, if a high logie level was desired at the motor eontroller, then a zero volt 
signal to the PWM-generating eireuit was required. Of note, although the LM6132 “rail- 
to-rail” opamps were used to buffer the PWM speed signals, a quad LM324 opamp was 
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used to buffer the direetion signals to reduee the number of eomponents on the PCB and 
save spaee. This ehoiee proved adequate, but the voltage of the high logie state was 
observed to reaeh a mere 3.7 V, not 5.0V. Regardless, the signals’ voltages were 
suffieiently high to properly operate the motor eontroller. 

The eomplete eireuit is provided in Appendix A. It shows the three motor 
ehannels, optieal isolators and unity gain amplifiers installed to buffer the output. The 
PCB for the three ehannel eireuit was laid out using Cadsoft’s Eagle eireuit design 
software, whieh produeed all the needed Gerber and drill fdes for manufaeturing. The 
PCB was installed under the Main Power Panel (MPP) to limit the length of the motor 
power bus leads. The optoisolated PWM signals leave the PWM board and are routed to 
the underside of the ehassis as a RFI reduetion measure. 

C. ELECTRICAL POWER SYSTEM 

1. Design Goals 

The eleetrieal power system was designed to be funetional, reliable, expandable, 
and eompaet. Part of funetionality and reliability was the desire to provide the CPU and 
eleetronies with supply voltages that were noise free. At the earliest stage of design, it 
was deeided that separate power supplies for motors and eleetronies were neeessary to 
aehieve low noise on the eleetronies supply. No previous SMART program robot had 
implemented isolated battery power supplies [9,10,11]. Redueing or mitigating RFI was 
a reliability eoneern, sinee noise supply voltages eould interfere with reliable operation of 
sensitive mieroeontroller deviees. Fabrieation of eleetrieal bus eomponents relied heavily 
on handmade printed eireuit boards to improve reliability by eliminating wire runs that 
eould aet as antennas for RFI. PCBs also helped aehieve neat, eompaet eomponents that 
fit the Creature’s spaee eonstraints. Power eonneetors in exeess of the planned number of 
loads were ineluded in the design to provide expandability. 
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2. Electrical System Design Evolution 

The Creature’s electronies power supply underwent three major revisions. The 
earliest power bus included a forerunner of the current MPP. Constructed using heavy 
barrier strips to connect to the robot’s three batteries, the first bus’ 700 g mass and large 
size rendered it unsatisfactory when the battery arrangement was changed. The Main 
Power Panel, constructed from printed circuit board materials, followed. 

Initial mockup of the Creature’s major components called for three identical 12 V 
Nickel Metal Hydride (NiMH) batteries to be symmetrically mounted under the chassis. 
Two were planned to supply power to the three DC motors, and one was to provide 
electronics power. This elegant arrangement ensured the center of mass coincided with 
the chassis’ center. The linear regulators responsible for the twelve-volt power required 
roughly two volts headroom above the regulated voltage, though, and the original 
electronics battery was not able to provide sufficient voltage. The NiMH battery was 
removed and an extensive redesign of the electrical system and chassis ensued. 

Experience with earlier SMART program robots indicated the largest electronics 
load would be the robot’s wireless communications device [11]. The current demand of 
the two routers available to the researcher was anticipated to be of the order of one 
Ampere, and this relatively large current demand ultimately caused problems with the 
first-generation electronics bus. A second-generation electronics power bus and a new 
buck switching DC/DC regulator were designed to address the shortcomings of the first- 
generation bus. The 5V DC/DC regulator section provides further details regarding the 
theory and construction of the buck switching regulator. 

Appendix B provides an index to numbered electrical supply lines, digital signal 
lines, and analog signal lines that are installed in the robot. Color-coded wires also have 
been numbered for ease of reference. Consult Appendix B for color codes as well. 

3. Overview of Motor and Electronics Power Busses 

The Creature’s electrical power distribution consists of an electronics power bus 

and a completely isolated motor power bus. This isolation was implemented to prevent 

noise from the DC motors producing noise on the power and ground lines of sensitive 
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electronics such as the CPU or microcontrollers. Figure 15 shows a simplified block 
diagram of the major components of the robot’s electrical system. Yellow components 
represent electronics bus components, while green is used for motor power bus devices. 
The following components have separate electrical supplies and different ground voltage 
references; 

• Main Power Panel (MPP) 

• Pulse Width Modulation (PWM) and Optoisolation Circuit 

• Test and Distribution Panel 



Figure 15. Simplified Electrical System Diagram. 
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4. 


Motor Power Bus 


The motor power bus is expressly designed to provide unregulated voltage to 
three loads, namely the three DC motors for propulsion. It has a seeondary function to 
provide 5V DC power to the optically isolated components in the PWM circuit that must 
be isolated from the electronics power bus. To the maximum extent possible, all wiring 
runs for the motor power bus are located underneath the chassis’ disk. Where 
connections must be made to components atop the chassis, such as the Test and 
Distribution Panel or Main Power Panel, the length of the leads on the upper side of the 
chassis has been minimized by routing them under the chassis to points near the 
component before routing the wires back up through the chassis. The motor power bus 
consists of four components: 

• Batteries 

• Motor Battery Interconnect and Charging Panel 

• Main Power Panel (MPP) 

• Test and Distribution Panel (TDP) 

a. Motor Power Batteries 

Two NiMH batteries with Anderson Power Products (APP) Powerpole 
connectors are connected to a motor battery interconnect panel mounted under the 
chassis. The APP connectors are color-coded red positive and black negative. The 
connectors provide secondary electrical isolation; the interconnect panel provides the 
primary electrical isolation. Each twelve-volt battery consists of 10 C-cells, and has a 
4000 mAh capacity. Each battery’s mass is 772 g. Eully charged, each battery has been 
observed to possess roughly 12.8 to 13 V potential between terminals. 

b. Interconnect and Charging Panel 

The interconnect and charging panel’s function is to allow for independent 
charging of the two batteries. The panel is a printed circuit board (PCB) with a 1 oz 
copper layer using 0.15 inch traces. It mounts two DPDT switches. Wide traces were 
used to allow for expected current flows of approximately 4 to 6 A. Eigure 16 shows a 
schematic representation of the panel. 
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Figure 16. Interconnect and Charging Panel Schematic. 

On the left of the diagram, the connections represent screw terminals 
which, in turn, are connected to short intermediate leads with Anderson Powerpole 
connectors to interface with the two batteries’ leads. When the panel’s series switch is 
closed, it allows the operator to put the batteries in series, providing 25 to 24 V, 
depending on battery charge. The panel’s charge switch connects the batteries in series 
to the motor power bus. Opening the connect switch disconnects the batteries from the 
bus for independent charging if the series switch is opened, too. The panel includes 
“banana” jack connections for connecting a battery charger to each battery without the 
need to remove the motor batteries or disconnect battery leads. 

c. Main Power Panel (MPP) Motor Power Bus Section 

The MPP is a PCB mounted atop the chassis. Its primary function is to 
switch the main electrical supplies and provide current protection with fuses. Referring 
to Figure 15, one can see the board contains a motor power bus section and an electronics 
power bus section. The following description refers to the panel’s operation with respect 
to the motor power bus. Function of the panel with respect to the electronics power bus 
is addressed separately. Figure 17 shows a schematic of the motor power bus section of 
the MPP. Although collocated on the same PCB, electrical isolation demands that the 
components not share any common leads or traces, and the traces were intentionally 
grouped to increase separation. To properly handle the expected currents to the motors. 
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the motor traces were kept wide, typically 0.1 inch. Although 1 oz copper PCB material 
was used in the board’s construction, 2 oz material would be preferred to increase the 
current carrying capacity and limit Ohmic heating. Two 330 pF capacitors, labeled Cl 
and C2 in Figure 17, limit noise on the unregulated series voltage output. 
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Figure 17. Main Power Panel Schematic. 
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The MPP includes a double pole double throw (DPDT) switch for the 
Motor Power Bus. Closing the switch connects the unregulated series voltage input from 
the interconnect and charging panel to the three motor controllers via three independent 
fuses, one for each motor. This is a precautionary measure and provides for future 
growth/change, since the installed motor controllers contain on-board fuses. Alternative 
motor controllers could be used in the future without the need to redesign the robot’s 
power bus. Presently, 2 A fuses are installed, which limits current well below the 3 A 
operating limit of the motor controllers. An LED indicator lights when the motors are 
receiving DC voltage. Opening the motor switch opens the circuit, removing the series 
voltage and allowing the operator to safely replace fuses. DC voltages for the motor 
controllers are output through screw terminal connections. 

A secondary function of the panel is to provide five volt DC power to 
components in the PWM circuit that must be electrically isolated from the electronics 
power bus’s five volt bus. To accomplish this, the main power panel mounts one five- 
volt LM7805 linear regulator in a TO-220 package. It is heatsinked, although the current 
draw by the PWM circuit’s components does not demand it. The MPP has screw 
terminal connections for connecting its regulated five volt supply, unregulated motor 
battery series voltage, and 0 V ground reference to the Power Distribution and Test Panel. 
Motor power is supplied directly to the individual motor controllers. 

d. Test and Distribution Panel 

The Test and Distribution Panel contains sections for the motor power and 
electronics power busses. The panel’s function is to provide easy access to motor bus 
voltages for testing and troubleshooting. Additionally, the panel contains four banks of 
WAGO connectors. Two of the banks are connected to motor power bus sources and 
allow the user to supply electrical power to devices installed on the robot. The motor 
power bus’ primary loads, the three DC motors, receive their supply separately, directly 
from the MPP. The bus has only one other load. It supplies five volts to a portion of the 
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PWM and optoisolation circuit containing the 2531 optoisolators and opamps that are 
detailed in the seetion deseribing the PWM eireuit. Figure 18 shows a diagram of the test 
and distribution panel. 
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Figure 18. Test and Distribution Panel Sehematie. 


5. Electronics Power Bus 

The eleetronies power bus eonsists of seven eomponents: 

• PowerStation 100 Eleetronies Battery 

• AC Power Supply 

• Main Power Panel 

• 12V Regulator Panel 

• Test and Distribution Panel 
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• 5V Electronics Bus Panel (First-generation) 

• 5V Electronics Bus Panel (Second-generation) 

• 5V DC Buck Switching Regulator Panel 

Consulting the simplified system diagram presented in Figure 15, one should note 
that the electronics power bus loads fall into four major categories. Eoads with sensitive 
analog components such as the Inertial Measurement Unit (IMU) operated best with 
precise 5.0 V supply. Other devices such as the sonar sensor head controller, namely a 
PIC microcontroller, can operate on a range of voltages around 5.0 V. A third class of 
loads, such as the wireless router, require five-volt regulated supply, but can function 
with small variations, e.g. 0.1 to 0.2 V, about 5.0 V. The fourth class of loads includes 
loads that have on-board voltage regulation. The BE2600 SBC and RabbitEink 
programmer are capable of self-regulation of the supply voltage. 

The table below lists the installed electronics devices, their supply voltages, the 
observed current demand measured at the output from the electronics bus during normal 
robot operation, and the power required based on the equation below. The power 
required by each device, P, is equal to the measured current, I, multiplied by the voltage, 
V. 

P = IV 


Device 

Current (mA) 

Voltage (V) 

Power (W) 

Router 

700.0 

5.1 

3.6 

BE2600 

210.0 

12.0 

2.5 

RabbitEink 

75.0 

16.5 

1.2 

Sonars & PIC controller 

215.0 

5.1 

1.1 

Sonar Sensor Head Servo 

180.0 

5.1 

0.9 

IR Rangers (6) 

159.0 

5.0 

0.8 

PWM 

61.0 

12.0 

0.7 

IMU 

96.0 

5.0 

0.5 

Tachometer & Detectors 

61.0 

5.0 

0.3 

I2C Compass 

8.8 

5.0 

0.0 

I2C SCE & SDA 

0.4 

5.0 

0.0 


Table 1. Measured Electronics Bus Eoads. 
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a. 


Electronics Battery 


The original electronics battery was replaced with a PowerStation 100 
lithium ion battery manufactured by Total-micro. It was designed to provide DC voltage 
to laptop computers consuming 60 to 75 W. According to the manufacturer, the battery’s 
specification calls for it to provide 16 to 20 V under 6.6A load, which agrees with its 
nominal 16.5 V rated voltage [22, 23]. According to its specification, its rated capacity is 
6.6 Ah [23]. Its mass is 907 g, and it measures 0.7 cm x 21.8 cm x 29.0 cm. The battery 
is connected to the electronics power bus section of the MPP using the manufacturer’s #6 
adapter plug, which provides center positive and exterior ground. 

b. AC Power Supply 

A Sony AC-V018G AC to DC adapter was found to supplement the 
PowerStation 100 battery. If prolonged stationary testing of the robot’s sensors or CPU 
is required, the operator can disconnect the electronics battery and connect the AC power 
supply in its place to preserve the battery’s charge. The device’s case states that it is 
rated at 18V and 4A, 72W maximum. Its original connector was replaced with a plug 
matching the PowerStation’s #6 plug, with a center positive and exterior ground. 

c. Main Power Panel (MPP) electronics bus section 

The Main Power Panel contains an electronics bus section and a motor 
power bus section. This paragraph describes its functionality with respect to the 
electronics bus. The panel contains switched fuses to provide current limiting for 
electronics. The panel contains two DPDT switches. Figure 17 shows a schematic of the 
MPP. The electronics battery’s leads are connected via screw terminal connectors to the 
panel. The unregulated battery voltage is constantly connected to the 12 V Regulator 
Panel. When the CPU switch on the MPP is closed, the 12 V Regulator Panel’s output 
passes through a fuse and is connected to a screw terminal ELEC_OUT-l. Erom the 
terminal the regulated twelve volts is routed to the Test and Distribution Panel. An EED 
lights when the switch is closed to indicate the CPU is receiving power. A second 
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switch, labeled 5VBus is loeated between the CPU and motor switehes. When it is 
elosed, unregulated power from the eleetronies battery is allowed through the fuse and 
routed to serew terminals for eonneetion to both the 5V/12V Bus Panel and the Test and 
Distribution Panel. An LED operates to indieate power is available to the bus panel. 

d. Test and Distribution Panel 

The Test and Distribution Panel eontains seetions for the motor power and 
eleetronies power busses. This seetion deseribes its operation with respeet to the 
eleetronies power bus. The panel provides easy aeeess to eleetronies bus voltages for test 
and troubleshooting. Additionally, the panel eontains two banks of WAGO eonneetors 
on its eleetronie power bus side. One provides unregulated voltage (nominally 16 V) 
from the eleetronies battery, and the other provides twelve-volt power. The CPU is the 
only twelve-volt load eonneeted to the panel. It is switehed via the MPP’s CPU switeh. 
Unregulated voltage is available to the panel’s WAGO eonneetors when the MPP Bus 
switeh is elosed. The RabbitLink Ethernet programming interfaee for the BE2600 CPU 
is the sole load for unregulated voltage from the Test and Distribution Panel. It reeeives 
its supply via the third generation 5 V Eleetronies Bus. 

e. 12 V Regulator Panel 

Although most eleetronie deviees on the robot required five volts, the 
robot’s CPU and original router required twelve-volt power. The spaee available on the 
ehassis and the need to reserve the area over the eenter of the robot for the IMU required 
a separate panel for produeing regulated 12 V. A eompaet PCB was built to mount one 
SGK EM7812 linear regulator in TO-3 paekage, a heatsink, and serew terminal 
eonneetors. The PCB ineluded a generous ground plane on the reverse to reduee noise. 
It was positioned to minimize the length of the leads eonneeting it to MPP. The MPP 
supplies a nominal 16 V from the eleetronies battery to the regulator, and the regulator’s 
output is returned to the MPP where it is switehed. Eigure 19 shows a photograph of the 
panel. The edge of the MPP is on the left, and the 12 V Regulator Panel is in the eenter. 
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Figure 19. Twelve Volt Regulator. 


f. 5V Electronics Bus Panel (First-generation) 

Initially, the Creature’s electronics were planned to use exclusively five- 
volt devices. The first-generation bus panel provided supply voltage and ground 
connections for electronic devices installed on the Creature. In keeping with the 
electrical system design goals of expandability and reliability, a PCB was constructed to 
mount four LM7805 linear voltage regulators and seven banks of WAGO connectors. 
LED indicator lights were omitted to reduce voltage drops. The number of WAGO 
connectors was chosen to provide numerous unused connections for future loads. The 
output of each of the regulators is switched. The sonar modules can draw momentary 
peak currents of approximately 1 A during transmit [24]. The SFIARP infrared range 
sensors were selected to share the regulator with the sonar modules because of the IR 
sensors’ small current consumption. Devices using the I2C bus were teamed to receive 
five-volt power from a single regulator. Due to the large current demand of the wireless 
router it was provided its own linear regulator. 

The completed first-generation bus panel proved inadequate and required 
a redesign. The LM7805 regulator failed to maintain voltage when loaded by the 
wireless router. The board’s layout impeded easy access to three of the banks of WAGO 
connectors. Its usability was mediocre; the placement of the WAGO connectors did not 

allow users sufficient room to easily connect wires. 
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g. 5 V DC Buck Switching Regulator Panel 

The failure of the first-generation electronics bus to maintain its live-volt 
output when under load prompted the researcher to address the root of the problem and 
develop a robust five-volt supply. In keeping with other work on the Creature, efforts 
were made to ensure the solution was reliable and extensible to current and future 
robotics projects in the SMART program. Noting that the original LM7805 had been 
producing excessive heat, in effect wasting battery power to produce heat as a byproduct, 
the researcher decided to pursue a more efficient DC/DC conversion method. Switched 
power supplies are commonplace in all manner of battery-operated devices, from laptop 
computers to cellular phones, but none had been installed previously on a SMART 
program robot. Equation 3.1 from Pressman gives the linear regulator’s efficiency [25]. 
Equation 3.2 gives the efficiency of a buck switching converter [25]. 


P VI V 

Series Pass Device Efficiency = —^ ” ° = —^ 

P V, I V, 

in dc o dc 


Buck Switching Converter Ejficiency = 


P„ + DC losses + AC losses 


(3.1) 

(3.2) 


Implementing a simple step down, or buck switching, DC/DC converter 
could result in drastically improved efficiency as demonstrated below, where V is the 
voltage, P the power, and I the current. If one assumes a 1 V diode drop across the diode 
and a worst-case AC switching loss case, then the losses are [25]; 

DC losses = = 14 

AC losses = 2 VJ„ ^ 

And the DC/DC converter efficiency becomes [25]; ; 


Buck Switching Converter Ejficiency = 




V 


VJ.+V,,„J„ + 2V,J 


O rj^ 


K. + 1 + 2V.. 
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Consider an example where the linear regulator’s only load is a wireless 
router, drawing eurrent equal to 0.7 A, as in Table 1. If the linear regulator’s negligible 
quieseent eurrent is negleeted, then lout equals Tn. The effieieney reduees to a ratio of the 
output to the input voltage. In the ease of the first-generation eleetronies bus, stepping 
down an input voltage of 16V to 5.0 V gives an effieieney of 31%. In eontrast, a buek 
switehing regulator operated at 22 kHz with a switehing delay, Ts, equal to 0.3 ps would 
step down the same input voltage with effieieney equal to 82%. Note, Pressman’s 
treatment assumes the voltage drop aeross the diode is 1 V. Using a Sehottky diode, the 
voltage drop Vdiode would deerease from 1.0 V to roughly 0.6 V, and effieieney would 
inerease. 

Whereas a linear regulator operates a series pass transistor in its linear 
region, effeetively using the deviee as a variable resistor, a buek switehing regulator 
operates on an entirely different prineiple. As the name implies, a switehing regulator 
rapidly switehes a fast-operating transistor on-and-off For a eonstant period, inereasing 
the switehing duration of the on portion of the period produees a pulse width modified 
duty eyele. The time average of the voltage output inereases with the inereased on period 
time. To smooth the switeh’s square-wave output, an induotor-eapaeitor (LC) filter is 
used. The LC filter outputs are “ripple-free DC voltages equal to the average of the duty- 
eyele-modulated raw DC input....” [25]. Alternatively, one ean think of the induetor as 
storing energy in its magnetie field while the switeh, or transistor, is elosed. When the 
switeh is opened, the magnetie field’s energy is released into the load [26]. 

Figure 20 shows a eireuit diagram of an example buek switehing regulator. 
The MOSFET Q1 serves as the switeh. The induetor Lo aets in eoneert with the eapaeitor 
Co to filter the switehed voltage. 
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Figure 20. Example Buck Switching Converter. (After Pressman) 

Assume an ideal case where the voltage drop across the MOSFET is zero. 
When the MOSEET acting as the switch is closed, the voltage potential across the 
inductor Eq equals Vdc- Vq. The change in current with respect to time is given by 
Equation 3.3. 


d±jv^ ( 3 . 3 ) 

dt L 

Since the difference between input and output voltages is a constant as 
well as the inductance, the current trough the MOSEET behaves in a linear manner 
producing the positively sloped current Iqi waveform (d) in Figure 21. One can see the 
current waveform (d) increase linearly until time Ton. After time Ton the switch is 
opened, turning off the voltage supply. 
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Figure 21. Buck Switching Regulator Waveforms. (From Pressman) 


While the MOSFET switch is closed, the increasing current predicted by 
Equation 3.3 creates an increasingly large magnetic field. When the MOSFET is opened 
at Ton, the magnetic field present in inductor Eo is still present; it cannot instantly 
disappear. Rather, it resists the change in current, but the open switch prevents any 
current from flowing from the battery through the MOSEET. Instead, the diode D1 
initially supplies an equal current 1 di shown as waveform (e) in Eigure 2E 
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The diode only allows forward current flow and the anode is at a zero 
potential with respect to ground. Thus, the cathode must be more negative than zero. In 
other words, the cathode must be at a negative potential equal to one diode voltage drop, 
Vdiode, below ground. The output voltage, Vo, is unchanged, so the potential across the 
inductor is the difference between the output voltage and the negative diode drop voltage. 
The difference is equal to Vo - (-Vdiode). Because the polarity has been reversed, a 
negative sign is introduced, and current through the inductor is now given by the 
Equation 3.4. 

^ ^ ~(K +^diode) /3 

dt L 

Now the current flowing through the diode linearly decreases with time at 
a rate proportional to the sum of the output and diode drop voltages and inversely 
proportional to the inductance. Let E be the highest current achieved with the switch 
closed and Iqi be defined as the lowest current when the switch is open. Combining the 
waveforms (d) and (e) in Figure 21 produces a current waveform (f) that varies by E - E 
about a center value E. 

A combined MOSFET and regulator IC, the L4964, by 
STMicroelectronics was selected as the basis of a DC/DC regulation circuit. It is a high- 
current device capable of supplying 4 A and able to produce a wide DC output voltage 
ranging from a preset 5.1 V to 28 V. It requires few external components, and its 
switching frequency is easily adjustable using an off-chip RC circuit up to a maximum of 
125 kHz. A toroidal 138 pH inductor by Coilcraft, model DMT-3-138-6, was selected as 
the regulator’s inductor. The toroidal design was favored because of the toroid’s inherent 
magnetic field shielding. Additionally, the inductor was selected to because its saturation 
current, 6.0 A, was greater than the expected maximum current produced by the 
regulator. 

The switching frequency, fjw, was chosen using the equation below which 
assists engineers in choosing inductors for buck switching supplies [27]. Based on the 
Creature’s electronics battery voltage, the maximum input voltage, Vm max , was set to 
16.5V, and the output voltage, Vout, was set to 5.1V. FIR, the inductance to current ratio, 
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was recommended to be 0.3, but lower values would produce a more efficient regulator at 
the expense of transient current response. For the Creature, an LIR equal to 0.3 was used, 
and maximum current, Imax , was set to 4 A. The appropriate switching frequency was 
determined to be 21 to 22 kHz. 


(V- -V )-V 1 

V in max out / out ^ 


L UR L 


The output capacitance depends on the output ripple voltage allowable. 
The peak current in the inductor is Ipeak and given by the equation below [27]; 


^peak ^max ^ inductor ^max 


(V. -V )-V 

V m max out ' oi 


1 1 


Based on the designed 4A maximum current and 22 kHz switching 
frequency, a peak current of 4.4 A was expected. Thus, the Coilcraft inductor above was 
deemed acceptable. 

The output capacitor, Co, in Pressman’s diagram, was chosen using the 
equation relating output capacitance to voltage overshoot, AV [27]. Given the values 
above and a desired voltage overshoot of 0.1 V, the output capacitance was determined to 
be 2600 pF. 

L-{Ipeakf 

" (AV + V^J-VJ 

The buck switching controller, toroidal inductor, output capacitors, 

Schottky diode, and ancillary circuit components are shown in Figure 22, which provides 

a circuit schematic of the DC regulator. A Motorola MBR1635 diode, a device designed 

expressly for applications such as this one, was chosen for its ability to pass the expected 

current. Four 330 pF capacitors in parallel were used in place of a large single capacitor 
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to reduce the equivalent series resistance. Although the design called for 2600 qF output 
capacitance, a larger voltage ripple was deemed acceptable in order to avoid the large 
series resistance that such a capacitor would have created had it been installed. 



Figure 22. DC/DC Buck Switching Regulator Schematic. 


h. 5V Electronics Bus Panel (Second-generation) 

Coincident with the construction of the DC/DC switching regulator, a 
replacement 5V electronics bus was designed and built by hand from PCB. Whereas the 
first-generation bus combined the voltage regulation and load switching functions on one 
PCB, the DC/DC buck switching regulator circuit required its own circuit board. Thus, 
the functions were separated between two PCBs in the second-generation bus. A side- 
effect of this decision was that after the DC/DC regulator was proven, duplicate PCBs 
could be made and used on any SMART program robot. In fact, this came to pass. 
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When the servo motors operating the Bigfoot robot’s meehanieal arm required a high- 
eurrent 6 V supply, a duplieate of the Creature’s regulator was pressed into serviee [28]. 

Although the Creature’s high-eurrent five-volt loads were eapable of 
operating off the DC/DC switehing regulator’s output, it was deeided to provide the more 
sensitive sensors, speoifieally those with sensitive analog deviees sueh as the IMU, a 
linear voltage regulator supply. The seeond-generation bus panel ineludes serew terminal 
eonneetions for the regulated voltage from the switehing regulator and for unregulated 
eleetronies battery voltage. Five banks of WAGO eonneetors, eaeh with eight eonneetion 
points, are provided for eonneeting eleetronies loads. The first-generation bus’ bulky 
DPDT switehes were replaeed with single pole double throw (SPOT) ones, allowing the 
board to mount seven switehes in a more eompaet layout. Defieieneies noted in the 
earlier board were fixed, e.g. seating wire leads into the WAGO eonneetors ean be easily 
aeeomplished due to the WAGO’s orientation. A sehematie of the seeond-generation 
eleetronies bus panel is provided in Figure 23. 
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D. MICROCONTROLLERS 


1. Z-World BL2600 Single Board Computer 

The Creature’s operating program runs on a Z-World BL2600 44.2 MHz single 
board computer (SBC). The computer is based on the Rabbit 3000 microprocessor, a 
descendant of the Zilog Z-80. It has been selected for use in other SMART program 
robots because of its ease of programming with the vendor’s feature-rich, proprietary 
Dynamic C language and IDE [II]. The BL2600 includes three serial ports capable of 
being configured for RS232 communications, and a 10/100 BaseT Ethernet connection 
[29]. The microprocessor has 512 kB static RAM and 512 kB flash memory. Its 36 
digital I/O are divided into sixteen digital inputs, sixteen software-configurable as input 
or output, and four high current outputs. The BE2600 offers several jumper-selectable 
options for configuring its I/O. The installed BL2600’s J1 has been set to use an external 
5V pull-up voltage from the electronics power bus. In addition to digital I/O, the SBC 
has twelve analog I/O pins, eight of which are eleven-bit A/D inputs. Most importantly, 
the BE2600 has four twelve-bit D/A outputs. 

The BE2600 is connected to the sonar sensor head controller PCB via serial port 
C, which uses a three-wire RS232 arrangement consisting of a connection to ground, 
TxC, and RxC. The CPU is also connected to the IMU via a serial port E. It uses a three- 
wire RS232 and is connected to ground, TxE, and RxE. The table below provides a list of 
the pinouts used by the installed BE2600, their function, and the wire label number. I2C 
serial communications are bidirectional, and the BE2600’s configurable digital I/O 
channels, DIOOO and DIOOl, are configured as outputs. Although configured as output 
pins, the value of the pin can be read using library functions in Dynamic C. Because they 
are open collector, the outputs float to the 5 V pull-up voltage when not held low. An 
external 5 V pull-up voltage is supplied to the BE2600 via the +K pin. The BE2600’s 
jumper, JPl, is set to provide open collector output using the five-volt power. This five- 
volt power and ground are supplied via Second-generation Electronics Bus’s I2C switch. 
The digital output grounds are connected to ground via leads to the same circuit. The 
connection of the SHARP IR rangers includes two ground connections to ground on the 
IR switched portion of the Second-generation Electronics Bus. 
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BL2600 Pinout 

Funetion 

Wire Label Number 

DIOOO 

I2C Bus SCL 

41 

DIOOl 

I2C Bus SDA 

40 

DIO02 

Motor 1 direetion 

46 

DIO03 

Motor 2 direetion 

47 

DIO04 

Motor 3 direetion 

48 

AVO 

Motor 1 analog speed 

21 

AVI 

Motor 2 analog speed 

22 

AV3 

Motor 3 analog speed 

20 

AINO 

IR 1 analog input 

1 

AINl 

IR 2 analog input 

2 

AIN2 

IR 3 analog input 

3 

AIN3 

IR 4 analog input 

4 

AIN4 

IR 5 analog input 

5 

AIN5 

IR 6 analog input 

6 

+K 

5V pull up voltage 

D 


2. Z-World BL2000 Single Board Computer 

The BL2600 was not available initially in the Creature’s development, and the 
robot was designed from the start to use the BL2000, whieh has two analog voltage 
outputs. Herkamp provides an explanation of its eapabilities [11]. It was deeided early 
in development to use an analog voltage signal as the speed signal for eaeh of the three 
motors. It was believed that doing so would reduee CPU workload that would otherwise 
need to be devoted to produeing three PWM signals. Pressing the BL2000 into serviee as 
the Creature’s CPU resulted in an interesting problem that had not oeeurred previously in 
SMART program robots that used two analog motor speed signals for skid steering. 
Before motion testing eould eommenee, a method of produeing three analog signals 
needed to be eoneeived. To this end, a seeond BL2000 operating as a slave CPU was 
eonneeted via RS435 for the express purpose of produeing an analog signal direeted by 
the master BL2000. This arrangement proved adequate, but a better solution was pursued 
during development. Installing the BL2600 redueed power eonsumption vis-a-vis the 
two CPUs and removed the failure mode of a eommunieations loss between the CPUs, 
whieh would have resulted in restrieted or possibly uneontrollable motion. 
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3. Microchip PIC16F690 

Two devices were constructed and installed on the Creature using the PIC16F690 
microcontroller manufactured by Microchip Inc. PICs are installed in the sonar sensor 
head controller and the wheel tachometer. This section describes the microcontrollers. 
For a description of the sensor devices, see the following section. The PIC16F690 was 
selected for its ease of programming, product support, on-board serial port, numerous 
pieces of example code, and widely available documentation. Although the PICs were 
programmed in assembly language, C language compilers were available. 

Before developing a PIC microcontroller-based solution, a sonar controller PCB 
was designed using IC logic gates, binary counters, etc. Its component count exceeded 
twenty ICs, though, and its communications scheme was non-standard. A better solution 
using a microcontroller to reduce component count was sought. An example of the 
savings in component count is the wheel tachometer. In the three-wheel tachometer 
circuit a single PIC was used to replace an older two-wheel circuit that required over 
eight ICs. Also, a microcontroller solution provided flexibility in both devices’ 
implementations. If changes to a device were needed, the device could be reprogrammed 
to optimize it for another mode of operation. For example, operating parameters of the 
sonar, such as maximum range and time delay between sonar ranging attempts could be 
adjusted for outdoor use by easily by modifying the code rather than building a new 
hardware solution. In addition, the PIC’s built-in counters provided the researcher with a 
level of timing accuracy unmatched by the BL2600, which can only time events to 1 ms 
accuracy. 

The manufacturer. Microchip Technology, provides a comprehensive manual on 
the device’s operation, I/O, capabilities, and assembly language syntax [30]. Of note, the 
devices have three built-in counters, one with sixteen bit range. Operated at 8 MHz and 
1:1 clock prescaling, the internal counters offer 0.5 ps timing accuracy. The PIC16F690 
has a PWM module, a capture and compare module, and a built-in serial UART. The 
serial port can send RS232 signals via a level shifting IC or I2C communications. 
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Both PIC16F690 microcontrollers installed on the robot are operated at 8 MHz 
using the mieroeontroller’s built-in oseillator. The sonar sensor head eontroller eireuit 
uses one PIC16F690 eommunieating via RS232 at 57.6 kBd to pass its range data 
sentenee. The wheel taehometer mieroeontroller uses I2C bus eommunieations to pass its 
wheel speed data to the CPU when requested. The PICs’ programs, in Mieroehip PIC 
assembly language, were written and eompiled in the MPLAB IDE running on a Dell 
laptop. A PIC Kit II programmer was eonneeted between the programming laptop and 
the PIC to load the programs using Microehip’s In Cireuit Serial Programming (ICSP), 
whieh allows the operator to program the mieroeontroller without need to remove it from 
the eireuit board. 

E. COMMUNICATION 

Communieation with the Creature is aeeomplished using 802.1 Ig wireless LAN 
signals. Two wireless routers have been used on the Creature. In early motion testing, 
the CPU’s robot operating program depended on Telnet eommands for manual eontrol 
while the researeher verified the motion portion of the Dynamie C eode and the ability to 
produee motion as predieted in Chapter IT Initially, a Netgear Rangemax 240 router was 
installed under the tray holding the eleetronies battery. It’s operation was problematie 
and eventually deteriorated to the point that the Telnet session eould not be maintained 
for periods longer than roughly 30 s when transmissions were sent. Telnet sessions 
exeeeding eleven minutes were observed when no transmissions were sent, but 
preserving the Telnet session by not oommunieating was an unaeeeptable eondition. 
Investigation revealed that the Netgear router demanded exeessively high eurrent from its 
twelve-volt linear regulator during busy transmission periods. When the linear voltage 
regulator eould not mateh the eurrent demand output voltage to the router dropped and 
the deviee reset. 

The Netgear RangeMax router was replaeed with a D-Link DI-624 AirPlus 
Extreme router, operating on 5.0 V. The D-Link router’s AC power supply is rated at 2.5 
A. Sueh routers have been sueeessfully used on other SMART program robots with 
linear voltage regulators [10]. Smaller than the Netgear router, it was easily installed in 
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the same loeation under the electronies battery tray. The router retains its original 
antenna, which has proven completely sufficient in testing to date. Its mass was found to 
be 0.3 kg. 

The D-Link router was observed to draw 600 to 740 mA when operating and 
communicating via 802.11 g. To accommodate this it was the sole load for one LM7805 
live-volt regulator on the robot’s first-generation electronics bus. The D-Link router 
operated successfully for roughly eight weeks. Unfortunately, the heatsink attached to 
the LM7805 proved inadequate, and the router’s power demand caused the linear 
regulator to heat excessively. It was believed that the heating triggered the regulator’s 
internal thermal protection. During failures, the linear regulator’s output was observed to 
drop below 4.90 V, which forced the router to reset itself. A long-term solution to the 
router’s current demand is described in the switching regulator section. 

F. ENVIRONMENTAL SENSORS 

1. SHARP IR Range Sensors 

The Creature was designed for semiautonomous operation, which required the 
ability to sense obstacles in the environment surrounding it. Considering the cluttered 
intended operating environment, the researcher chose environmental sensors based on 
two criteria; close minimum range, and rapid update rate. The Creature needed to be 
capable of exploiting its holonomic mobility with sensors that could detect obstacles as 
close as the periphery of the robot. The Creature was expected to operate at up to 1.0 m/s 
maximum. The combination of speed and cluttered environment dictated that sensors be 
fitted that could provide a complete scan at roughly 1 Hz to allow the CPU sufficient time 
to detect, slow, and avoid an obstacle. The 2Y0A02 SHARP IR ranging sensor was 
seriously considered because of its detection range, approximately 18 to 100 cm, and its 
quick refresh rate, which was observed on an oscilloscope be 40 ms. The analog output 
displayed discreet steps when a target at 15 cm was quickly removed from the FOV. 

Herkamp provides an explanation of the operation of the SHARP IR ranger [11]. 
They are simple to use and produce an analog voltage inversely proportional to the range 
of the target. The sensor has an extremely narrow FOV that is 4 cm wide at 100 cm 
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range [31]. At r equal to 100 em with s equal to 4 em, the angular FOV, 0, would be 
0.040 radians. The number of samples, N, needed to sense the obstaeles at 100 em, using 
0 equal to 40 milliradians, is given by the Equation 3.5: 




2n 

~0 


(3.5) 


It would have required 120 samples to eompletely sense the 360° surrounding the 
robot without gaps. The number of samples posed a problem, but solutions using eight to 
ten deviees atop a seanning a deteetor head were feasible. More troubling, the deviees 
showed signifieant variation in their analog output voltage when tested in the lab against 
objeets at a eonstant distanee. Speeifieally, the output voltage was observed to be 0.7 V 
when viewing a 0 0.5 ineh smooth steel rod at 80 em. The output voltage dropped to 
0.37 V when the smooth rod was replaeed by a 0 0.5 ineh threaded rod. A wooden meter 
stiek plaeed with its widest side normal to the sensor’s F0V produeed a higher output 
voltage, 1.0 V, despite being plaeed at 1 m. Based on these observations the IR rangers 
were deemed unaeeeptable for use as the primary environmental sensor, and the deeision 
was made to pursue ultrasonie sonar for this role. 


Though not employed as the Creature’s primary sensor, IR rangers were installed 
to provide a limited eoverage of the spaee below the sonar sensor head’s view. Figure 24 
shows the equilateral arrangement of the IR rangers. Two IR rangers are installed 
antiparallel to eaeh other and perpendieular to eaeh of the three motor mounts. The 
sensors are plaeed at a height approximately 6 em above the surfaee the robot is operating 
on. A total of six sensors are installed, numbered one through six per Figure 24. Sensors 
one and two aet as a sensor pair, as do three and four, and five and six. The F0Vs of the 
sensors eross at three loeations, approximately 10 em from the edge of the ehassis at an 
position midway between eaeh wheel. This arrangement provides a virtual bumper when 
the robot is driven in one of its three prineiple direetions: 60°, 180°, 300° measured 
eloekwise from the Y axis. 
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Figure 24. IR Ranging Sensor Installation. 

Experimental observation of aluminum soda eans in the lab provided threshold 
voltage levels that the robot operating program uses to indicate the presence of an object. 
These cylindrical targets were insensitive to target orientation, unlike the meter stick 
described above. At the intersection of the sensors, output voltage was observed to be 2.2 
to 2.4 V. A voltage of 1.4 V indicated the presence of an object at 50 cm from the sensor, 
or 33 cm from the front edge of the chassis. If the robot were moving along one of its 
principle motion directions, such an object might collide with the wheel on the side 
opposite the sensor. Threshold values were set based on observation of the robot’s 
performance while conducting its random walk algorithm. 

The six sensors’ analog outputs are connected to the CPU breakout panel. The 
panel’s function is to provide I2C bus connections and allow for easy CPU change out. 
For example, if future researchers install a different CPU, the IR rangers’ wiring can 
remain intact, and the new CPU will simply need to be connected to the breakout panel. 
The panel also provides the sensors with five volt DC voltage for operation. The IR 
sensors are switched via the 5 V Electronics Bus Panel. 
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2. Scanning Sonar Sensor Head 
a. Design Considerations 

The primary environmental sensor for the Creature is its scanning sonar 
sensor head. Although SHARP IR rangers had desirable characteristics, such as fast 
refresh rate and close minimum range, the inability to accurately report range and the 
large number of scans required to completely scan all azimuths prompted the adoption of 
a conventional sensor, ultrasonic sonar. The Polaroid 49.1 kHz monostatic electrostatic 
sonar transducer and 6500 ranging module, manufactured by Senscomp, has been the 
most widely used sensor in previous robotics research [32]. The transducer’s beam width 
is approximately ±15° [33]. Using Equation 3.5, with 0 = 18° or O.Iti, the number of 
scans, N drops to 20. 

The SensComp 6500 ranging module accepts digital initiation, INIT, and 
blanking inhibit, BINH, signals. It outputs a digital echo signal that goes to a high logic 
state when an echo is detected. Its range accuracy is a function of the timing accuracy 
between the time INIT is sent high and the time the echo signal goes to a high logic state. 
Previous robotics class work at NPS had demonstrated centimeter accuracy using digital 
timing devices coupled to the SensComp 6500 modules [34]. This range accuracy was 
superior to that achievable using the SHARP IR rangers’ analog outputs. 

The researcher did not have a sufficient quantity of transducers and 
ranging modules to permit mounting eighteen fixed devices around the periphery of the 
robot. Further, the Creature’s components and chassis did not afford enough space to 
mount so many fixed devices. It was decided to use fewer devices and scan them in 
azimuth, but this solution created added complexity. 

The sonar system consists of four components: a sensor head controller, a 
servo motor, four ultrasonic transducers, and four ranging modules. The scanning sonar 
sensor head needed to complete the following tasks: 

• mount the sonar transducers and mechanically scan them 

• regularly send the INIT pulse to multiple SensComp 6500 ranging 
modules 
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• time the interval between INIT pulse and reeeipt of eeho signal, i.e. the 

TOP 

• store TOP data 

• eommunieate TOP data 

b. Sonar Mechanical Mounting and Pointing 

Pour sonar transdueers were mounted to a square seetion of perf board, 
measuring three inehes on a side. Perf board was ehosen beeause of its light weight and 
pre-drilled holes, whieh allowed easy alignment of the orthogonally-mounted 
transdueers. To limit rotational inertia of the seanning head, eonstruetion emphasized 
light-weight parts. It was feared that a heavier head with greater rotational inertia would 
not aeeelerate and deeelerate to a stop preeisely enough when the head was seanned at 
high speed, roughly one degree per milliseeond. This would have eaused misalignment 
between the sonar transdueer’s beam pattern at the time it was fired and its intended 
ranging seetor. 

The transdueers and perf board were mounted to a Putaba S3003 RC 
servo, whieh provides the rotational motion under the eommand of the sonar sensor head 
eontroller. The manufaeturer states that the servo requires 0.23 s to move 60° when 
supplied 4.8V or 0.16 s to move 60° when supplied 6.0V. Equivalently the servo ean 
move at a rate of 261% and 375% at 4.8 and 6.0 V, respeetively. It ean generate 3.2 kg- 
em torque at 4.8 V [35]. The servo ineluded a eireular servo horn, or mount, with the 
numbered labels at 90° intervals. The servo positioned the flange to its rightmost limit 
when a 0.30 ms signal was applied to its position signal input. With the servo ease 
aligned with an imaginary Y axis, the Putaba servo’s number 1 mount point was observed 
to move to a position 90° to the right, or aligned with an imaginary X axis. The number 1 
mount point’s position was observed and reeorded for various eontrol pulse widths. The 
angular position of the number 1 mounting point and the pulse width obeyed the linear 
relation below, where T is the eontrol pulse width in mieroseeonds, and 0 is the angular 
position in degrees from far rightmost limit of the servo’s rotation. 

r=300//5 + fl0^V 

I r J 
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c. Function of PIC Microcontroller and Sonar Circuit 

All other tasks listed above were accomplished using a PIC 
microcontroller mounted to a purpose-built PCB, designed by the researcher using 
Cadsoft’s Eagle circuit layout software. The PCB was manufactured by Advanced 
Circuits. The PIC16F690’s program is explained in chapter 4, and while the construction 
of the PCB and the sensor head controller is described below. Briefly, the PIC was 
programmed to send the appropriate servo pulse to the RC servo to position it to the 
required azimuth. Four discreet digital output pins on the PIC were connected to the 
INIT signal inputs of the four SensComp 6500 ranging modules. The PIC was 
programmed to fire a sonar, and count the oscillations of its internal oscillator between 
the time the INIT signal was sent high and the time the echo signal was observed to go to 
a high logic state. The four modules’ ECHO signals were multiplexed to simplify the 
PIC’s timing operation. The four ECHO signals were combined using an XOR gate, 
which resulted in a virtual four input OR gate. Thus, when any one ECHO signal went to 
a high logic state, the combined echo signal input to the PIC also went to a high logic 
state and provided a signal to cease counting the internal oscillator, stopping the TOE 
timing. 

A MAX232 RS232 level shifting IC was installed in the circuit to shift the 
PIC’s serial data voltages to the RS232 standard. The ranging modules produce 
noticeable electronic noise on the supply, ground, and echo lines when the transducers 
are fired [24]. Two 1000 pF capacitors were installed in parallel to reduce noise on the 
PCB’s five-volt supply during sonar firings. Figures 25 and 26 show the circuit 
schematic for the sensor head controller. 

The servo sensor head controller is powered by the electronics power bus 
and uses five-volt supply. The PIC16F690 and SensComp 6500 ranging modules receive 
power via the Sonar switch on the Electronics Bus Panel. The RC servo requires five- 
volt power as well, and is separately switched via the Servo switch on the same panel. 
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Figure 25. Sonar Sensor Head Controller Circuit. 
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G. POSITIONING SENSORS 

I. Wheel Tachometers 

The wheel tachometer system consists of three binary optical encoder targets, 
three optical detectors, and a PIC16F690 microcontroller responsible for measuring the 
frequency of the signals produced by the detectors. Each optical target consists of an 
aluminum disk screwed into a mounting collar with a hole through its center. The collar 
was designed to allow the motor shaft to slide through its center. The collar was made to 
rotate with the motor shaft by two set screws. A 0 1.75 inch circular target printed on 
transparency material with 100 sectors, alternating black and white, was affixed to the 
aluminum disk with glue. There are 50 sectors of each color. The black sectors provide 
an optical target for a Hamamatsu P5588 photo detector, which produces a logical high 
signal when a black object is detected. The Hamamatsu photodetectors are designed for 
applications such as detecting the presence of paper inside a laser printer and are limited 
to ranges of 1 to 3 mm [36]. Figure 27 shows a schematic of the P5587 sensor, which 
produces a high output when a light colored target is passed in front. The diagram shows 
an example circuit and the internal components, including the LED input diode, current 
amplifier, Schmitt trigger, and output phototransistor, visible. The sensor incorporates 
built-in hysteresis to ensure the transitions between logic states are free from noise. Its 
open collector output requires a pull-up resistor Rl to pull the IC’s Vo output up to a 5.0 
V high logic state. 
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Figure 27. Flamamatsu P5587 photodetector circuit. (From Hamamatsu) 

As the motor shaft rotates so does the circular target, passing a continuous series 
of alternating black and white sectors in front of the Hamamatsu detector. In the P5588 
as the edge of a black to white sector passes the detector, the reflected light off the target 
is detected and the current amplified. On a high to low transition, if the detector’s 
forward current exceeds 10 mA, the high logic state is triggered and the transistor is 
switched allowing the optical detector’s output to be pulled up to 5.0 V. The effect of 
passing a repeating series of alternating sectors is a clock signal with frequency 
proportional to the motor shaft speed. With a 100 sector optical target installed the 
Hamamatsu sensor generated a clock signal with 50 cycles per shaft revolution, e.g. 50 
cycles per two n radians angular rotation. 

All three motor shafts have a target disk mounted to them. Three compact PCBs 
were built to mount the detector, pull-up resistor, and noise capacitor. These PCBs were 
located inside the three motor mounts. Figure 28 shows a photograph of one optical 
target and its detector inside a motor mount box. The detector PCB is on the right, and 
one can see the Hamamatsu photodetector slightly to the right of the center where it is 
mounted in close proximity to the target disk, approximately 1 mm. 
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Figure 28. Wheel speed optieal target disk and deteetor. 



By loosening the eollar’s set serews, one ean adjust the position of the target 
along the shaft’s axial direetion until it is deteetable by the Hamamatsu sensor, at whieh 
point the set serews ean be tightened. It was observed that eare must be taken when 
tightening the set serews to prevent the target disk from being tilted. If the target disk 
“walks,” then it will no longer be normal to the optieal deteetor, and portions of the disk 
will be farther from the Hamamatsu deteetor when they pass its FOV. Some seetors were 
observed to fail to register due to the target’s exeessive distanee from the deteetor. If the 
target is plaeed too elose to its deteetor, the two ean rub and result in a damaged target. 
The effeet is a target with a “dead” seetion where no eloek signals are generated. 

2. Inertial Measurement Unit (IMU) 

The Creature is the first SMART program robot to mount an IMU. Beeause the 
intended operating environment would not allow for reliable reeeipt of GPS signals, it 
was deeided to ineorporate an IMU to researeh inertial navigation. The Onavi Faleon GX 
was seleeted based on its eost, ease of integration with the existing robot CPU via RS- 
232, and its features. The Faleon ineorporates three orthogonally mounted linear 
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accelerometers and three angular rate sensors. The eombined output is continually 
reported via RS-232 in either ASCII or binary format. Immediately after power up, the 
FaleonGX allows the user to eonfigure its RS-232 baud rate, and the data update rate. 
The default RS-232 data rate is 9600 b/s, and the update rate IHz [37]. To deerease the 
time required for receipt of the complete IMU data sentence, the RS-232 rate was 
changed to 19.2 kb/s. The update rate was set to 20Hz in order to provide the BL2600 
with timely rotation rate data when turning, since the Creature was observed to be 
capable of rotation in place at rates exceeding 1807s. 

The IMU was mounted to an aluminum plate that provided added mass and 
served to damp out small amplitude vibrations. The plate and IMU were attaehed with 
Velcro to the chassis in the geometric center of the robot. The IMU was installed such 
that its positive X, Y, and Z axes were aligned with the robot’s X, Y, and Z axes, 
respectively. Velcro allowed for easy adjustment of the device to align it with marking 
on the chassis that indicated the direetion of the robot’s Y axis. The IMU’s Z axis 
eoineides with the robot’s Z axis, but aecording to the manufaeturer, the IMU’s positive z 
rotation is opposite to the sign eonvention used in the robot’s frame of referenee, i.e. 
positive z rotation about the IMU’s Z axis is eloekwise as viewed from above and would 
be considered negative rotation about the robot’s Z axis in the robot’s frame of reference. 
Figure 29 shows an illustration of the orientation of the IMU’s linear accelerometer axes 
and rotation sense of the rotation rate sensors. Note, that positive IMU rotation rate data 
about its Z axis corresponds to negative rotation in the Creature’s frame of reference 
introduced in Chapter II. 
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Figure 29. FalconGX IMU Axes. (From ONavi) 

The IMU’s linear aeeelerometer and rotation rate sensors are analog deviees 
whose output is eonverted to 10-bit digital representation by built-in ADC. The 10-bit 
data seheme assigns zero g, or 0.0 m/s , to a value midway between zero and 1023, i.e., 
equal to 511. Stationary in the lab, the IMU reported -1 g, or - 9.80 m/s , aeeeleration 
along the Z axis,. The reported data is a 10-bit representation of the IMU’s analog deviee 
readings, and it must be sealed appropriately if one desires MKS units. The equation 
below gives the neeessary eonversions for the reported linear aeeeleration, a 10-bit 
representation of the deviee’s full seale 4g range, to aeeeleration in MKS. Rotation rate 
must be sealed as well using the full-seale value of 3007s [37]. 

, . r ; 2 n r reportcd valuc - ml 

accelerationunls J= - 4g - 

V 1024 y V ,? y 
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rotation rate \ rad I s = - - - 

^ 1024 ) s [ 360° ) 


63 



Data are reported to the BL2600 in ASCII format, whieh allows easier 
interpretation by humans. It was deeided to use this format during integration and 
testing, despite the faet that binary eommunieations would deerease the total bytes, and 
thereby the time, needed to transmit the IMU’s data sentenee. 
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IV. SONAR SENSOR HEAD CONTROLLER MICROPROCESSOR 
ASSEMBLY LANGUAGE PROGRAM 

A. PROGRAM TASKS 

The hardware eomponents assoeiated with the sonar sensor head eontroller have 
been diseussed previously. To provide flexibility and reduee hardware eomponent eount 
a mieroproeessor solution was developed to trigger the sonar ranging modules and 
measure the TOP. This seetion deseribes the PIC16F690 mieroproeessor’s assembly 
language program. The mieroproeessor is responsible for the following tasks: 

• Sending individual INIT pulses to different SensComp 6500 ranging 
modules 

• Timing the interval between INIT pulse and reeeipt of eeho signal, i.e. the 
TOP 

• Storing unique TOP data for eaeh seetor around the robot 

• Communieating stored TOP data to BL2600 

B. SONAR CIRCUIT AND PROCESSOR TIMING EXPLANATION 

A timing diagram for two of the four sonar ehannels is provided in Figure 30. It 
shows two sequential sonar ranging attempts. In the first ranging attempt, the number 
four SensComp 6500 ranging module sets its ECHO signal high after deteeting a valid 
return eeho. In the seeond ranging attempt, the number three ranging module does not 
deteet a valid return eeho, so its ECHO signal remains low. In the first ease the sonar 
sensor head eontroller PIC saves the value of the sixteen-bit eounter when the eombined 
eeho transitions from a low to high logie state. After the value is saved, the timer is 
allowed to overflow to ensure the period between RC servo pulses is approximately 30 
ms. In the seeond, no eeho is deteeted, so the sixteen-bit timer’s value reaehes its 
maximum, and the timer overflows. The PIC treats this as a non-deteetion in its eode. 
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COMBINED ECHO 


Figure 30. Sonar Controller Timing Diagram. 


C. PROGRAM FLOW 

The program begins by eonfiguring various registers. The PIC is eonfigured to 
use its internal RC oseillator for the eloek souree, the eode sets its speed to 8 MHz. It 
eonfigures all inputs to digital and the appropriate inputs and outputs per the table above. 
TIMERl is eonfigured to inerement at a 1:1 ratio with the instruetion eloek, whieh is one 
quarter of the proeessor frequeney, 8 MHz. Thus, TIMERl inerements every 0.5 ps. The 
RS232 UART is eonfigured for asynehronous serial operation at 57.14 kbit/s, eight-bit 
data, with no parity. Eigure 31 shows a flow ehart that deseribes the mieroproeessor’s 
program. The program’s flow ehart is eontinued on Eigure 32. 

The program uses a 20 to 35 ms time period as the primary building bloek for all 
aetivities beeause the RC servo must reeeive its positioning signal at an interval on the 
order of 20 to 40 ms. After eonfiguration and initialization, all program aetions take 


66 

























































place within a regular 20 to 40 ms period or time bloek. The main loop begins by 
moving the servo sensor head to its initial position. The subroutine SubSendServoPulse 
is ealled to send the servo a high logie pulse. The pulse’s duration is proportional to the 
angular position desired. The subroutine uses the value of the position variable in a ease- 
switeh statement to ehoose the length of the high pulse. The RC servo signal must be 
repeated after roughly 20 ms, and the program ealls the subDEL20 subroutine to produee 
the delay. If the position is the initial position, then a longer delay eonsisting of eight 
iterations of the servo pulse and 20 ms delay is provided in the program to allow the 
deteetor head extra time to move from its final position to the initial position, an angle of 
IT. Based on the Futaba S2003 speeifieation rotation rate of 2617s, one would expeet 
the rotation from the last position to the starting, initial position to require 276 ms [35]. 
When the servo is only required to move 18“ between sonar firings, the servo pulse and 
20 ms delay is repeated just three times beeause the smaller angular displaeement 
requires less time for the servo to eomplete. 

After the servo moves to the eorreet angular position, the program eommands a 
sonar ranging. Eaeh of the four sonar transdueers is triggered sequentially beginning 
with sonar number four. The appropriate INIT signal output is taken to a high logie state, 
eausing the eorresponding SensComp 6500 ranging module to trigger its sonar ranging 
eyele, and the transdueer transmits sixteen pulses. The PIC’s enhaneed eapture and 
eompare module is eonfigured to trigger on the rising edge of the eombined ECHO signal 
and the assoeiated interrupt flags are eleared. Then TIMERl’s eount is eleared and the 
eounter started. 

The SensComp 6500 module has a default 2.38 ms delay after transmitting the 
sixteen sonar pulses to prevent eonfusing the transdueer’s ring down as a return eeho 
[24]. Assuming the speed of sound in air equals 343 m/s, this limits the minimum range 
to 80 em. It was deeided that this minimum range was unaeeeptably large for the 
expeeted operating environment. At the risk of deteeting the ring down as eeho the PIC 
overrides the default eeho blanking by sending the blanking inhibit BINH to a high logie 
state after 1 ms. The 1 ms BINH delay has been observed to provide a 20 em minimum 
range. 
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Figure 31. PIC16F690 Sonar Sensor Head Controller Program Flowehart. 
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Figure 32. Continuation of Program Flowchart. 

Then the PIC begins a loop that checks the interrupt flags for either receipt of an 
echo or a TIMERl counter overrun. If the SensComp 6500 ranging module detects a 
valid echo, it sends its ECHO signal high. The four module’s ECHO signals are 
combined via a XOR gate. If the combined echo transitions from a low to high logic 
state, both of TIMERl’s counter bytes are saved, and then TIMERl is allowed to finish 
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counting up until overflow. This ensures the proeess’ duration takes 33 ms regardless of 
the length of time between sonar firing and reeeption of the eeho, and it guarantees the 
RC servo pulse will be sent again after an appropriate delay. 

The most significant byte is tested to determine if the counter value indicates 
receipt of an echo from a target at a range that exceeds the limits of the reportable range. 
These TOP values are set to a literal value 0x02 to flag the data as no eontaet. The most 
significant byte and least signifieant byte of the eounter variables are combined to make a 
one-byte value of the eount. The 2’ bit through the 2^^ are retained, and the byte is saved 
to an array whose index eorresponds uniquely to the eombination of sonar number and 
servo position using the relation below. 


array index = 4(5 - position) + (4 - sonar number) 

The variable SonarNum is decremented to allow the PIC to complete a sonar 
ranging with each of the sonar modules from four to one. The inner loop is repeated. If 
deerementing the variable results in a value of zero, then all sonars have been fired and 
the position variable is deeremented. The position loop is repeated until deerementing 
the position variable results in a value equal to zero. Then the position value is reset to 
its initial value, five, and the outermost loop begins again with the sensor head being 
commanded to move to the initial position. 

D. PROGRAM OUTPUT 

The PIC transmits the one-byte counter representation of the TOP through its 
built-in UART. To ensure the robot’s BL2600 CPU is not delayed any longer than 
necessary, the PIC transmits all data available as part of the subroutine that is responsible 
for sending the RC servo pulse. Rapidly refreshing the data also ensures that the CPU is 
afforded many opportunities to reeeive the range data. There is no need for the CPU to 
interrupt its processing to read in the PIC’s transmission. The CPU ean expeet to reeeive 
the 20 range data bytes at approximately 20 to 33 ms intervals. The eommunieated 
sentence uses flags with hexadeeimal values that are outside the counter values for 
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minimum and maximum counts. Since the reported range is limited to 250 maximum, 
values 251 to 255 are available for the program’s use as flags. The minimum range 
should correspond to a count of 19 or 20, which allows values 0 to 18 for use as sentence 
flags. The hexadecimal value 0x02 indicates no echo was received or that the echo was 
detected beyond roughly 2.5 m. The values OxFE and OxFB indieate sentence start and 
end, respectively. 

Figure 33 shows an example data sentence from the sonar controller. The 
sentenee is ordered such that the first range byte corresponds to the sector aligned 
approximately with the robot’s Y axis. Moving clockwise around the robot, succeeding 
bytes represent the range for the next eighteen-degree sector. Thus the tenth data byte 
corresponds to the sector roughly aligned with the robot’s negative Y axis. After the 
twentieth data byte is sent, the PIC transmits an integer value between 0 and 19 to 
indicate which array value is most recent. 

\* -Range Data Bytes (shown as decimal values for clarity)-H 

193 211 02 188 167 166 230 200 93 94 93 167 170 172 02 158 , 3 V 

\ 

Most Recent Most Recent Sonar End Flag 

Sonar Range Reading Array ASCII OxFB 

Indicator 

Figure 33. Sonar Range Data Sentence. 



Two Start Flags 
ASCII OxFE 


Note the integer value uses the PIC’s indexing scheme, which corresponds to the 
sequence that the sonars were fired. The PIC stores the data bytes with an array index 
according to the order the sonars ranging attempts were made. Because the twenty 
azimuth scans are interleaved, the PIC’s internal data array index does not correspond to 
the same clockwise azimuth order that is reported. Translating between the two indiees is 
accomplished with a simple lookup table, and one can determine how time-late eaeh 
range is for any sector. 
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Sonar Sentence Order 

Azimuth (relative to Y axis) 

PIC Data Array Index 

0 

6“ 

2 

1 

24” 

19 

2 

42” 

15 

3 

o 

O 

II 

4 

oo 

o 

7 

5 

96” 

3 

6 

114” 

16 

7 

132” 

12 

8 

o 

o 

8 

9 

o 

OO 

4 

10 

o 

OO 

0 

11 

204” 

17 

12 

222” 

13 

13 

240” 

9 

14 

258” 

5 

15 

276” 

I 

16 

294” 

18 

17 

312” 

14 

18 

330” 

10 

19 

348” 

6 


Table 2. Sonar Data Array Index to Azimuth Cross-reference. 


If one assumes the robot travels in a straight line without rotating between 
received data sentences, then older range readings can be adjusted for robot motion by 
multiplying the robot’s velocity vector times the time since the sonar detection was 
received. The effect is to move all range readings to objects, except the current one, by a 
distance proportional to the apparent motion between the robot and the object. 

The following table lists the connections used by the PIC in addition to the In- 
Circuit Serial Programming (ICSP) pins. 
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Description 

PIC Connection 

IC Pin Number 

RC Servo Position 

RCO 

16 

Sonar 1 INIT 

RA2 

17 

Sonar 2 INIT 

RB6 

11 

Sonar 3 INIT 

RA5 

2 

Sonar 4 INIT 

RB4 

13 

Blanking Inhibit (BINH) 

RA4 

3 

Combined ECHO 

RC5 

5 

RS232 TX 

TX 

10 

RS232 RX 

RX 

12 

+5 V 

VDD 

1 

GND 

vss 

20 


Table 3. PIC Connections to Sonar Circuit. 
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V. THREE WHEEL TACHOMETER MICROPROCESSOR 
ASSEMBLY LANGUAGE PROGRAM 

A. DESIGN CONSIDERATIONS AND PREVIOUS WORK 

The functionality and flexibility of the PIC16F690 microcontroller prompted the 
researcher to develop a wheel tachometer based solely on it. Until the Creature, SMART 
program robots had not incorporated wheel speed sensors, nor been equipped to perform 
odometry calculations [10, 11]. Based on previous NPS robotics class work, the 
researcher was familiar with binary encoders and odometry using optical targets and 
detectors. A hardware solution had been built, tested, and demonstrated on a chassis 
similar to Herkamp’s, but this device was extremely restricted in its usefulness and used a 
very rudimentary three wire serial communications scheme to “bit bang” its data to a 
BL2000 [34]. Further, the device attempted to measure distance directly through a count 
of the encoder’s rotation. 

B. TACHOMETER PROBLEM SOLUTION 

A better solution needed to be found. Using the same type of optical targets and 
sensors, a more robust and flexible solution was devised using a single PIC16F690 in 
place of the PCB described above, which included seven ICs. The PIC allowed the 
researcher to use common I2C bus communications and added unparalleled flexibility 
through implementation of a software solution. With a software-based PIC solution, the 
tachometer can easily be modified to operate on other robots with two, three, or four 
odometry sensors. Further, the actual sensor is immaterial; the PIC can be adapted to 
work with any regularly occurring digital signal such as one from a Hall Effect sensor. 
The PIC sensor could be used to provide feedback about any motor shaft motion, such as 
motors used to position a robotic arm. This adaptability was a design goal throughout the 
development. Solutions should be flexible enough to allow for their implementation on 
follow-on SMART program robots. 
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Whereas the earlier deviee used hardware to measure displaeement, an angular 
veloeity measurement teehnique was seleeted for the Creature to smooth out the observed 
data. The mieroeontroller’s fundamental task is to eount the number of eyeles reported 
by eaeh sensor in a fixed period. The PIC16F690 used for wheel speed measurement 
aeeepts three inputs, one from eaeh Hamamatsu optieal sensor observing the Creature’s 
three motor output shafts. The sensors provide a digital signal that goes to a high logie 
state when the blaek portion of the binary eneoder wheel passes through its FOV. The 
PIC’s program eounts the number of transitions, i.e. ehanges from high to low or low to 
high logie states, in a fixed period of time. If one assumes that one eyele ineludes two 
transitions, dividing the eount of the transitions by two times the period gives the 
frequeney in eyeles per seeond of the motor shaft. A 250 ms period was ehosen as the 
sampling period. A longer period would have smoothed the frequeney data more and 
mitigated the observed optieal sensor errors due to oeeasional failures to deteet some 
target seetors. Additionally, deteeting low frequeney signals (10 to 17 Hz) assoeiated 
with very slow wheel speeds required a longer sampling time. Longer periods, though, 
obseured short-term variations in motor frequency, for example during accelerations. 
Ultimately, a 250 ms period was chosen as a compromise. Further, it allowed for a one- 
byte data value at the maximum expected signal frequency of 220 to 250 Hz, and 
simplified the assembly language mathematics, since multiplication by two consists of a 
simple bit shift. 

In addition to measuring the motor shaft frequencies, the PIC must also store the 
data and communicate it to the robot’s CPU, the BL2600. It was decided to use I2C bus 
communications for this application because the CPU’s serial ports were committed to 
sonar and IMU data communications. Rudimentary communications protocols outside 
the mainstream have been used on some earlier odometry sensors, but such protocols 
violated the design goal that devices built for the Creature be adaptable to a wider range 
of vehicles. Using the I2C bus protocol allowed the CPU, serving as bus master, to poll 
slave devices at its convenience. By using the I2C protocol the data was available to the 
CPU with minimal delay. I2C uses a simple bi-directional, two-wire hardware layer that 
was already installed, and the CPU’s Dynamic C language provides a feature-rich I2C 
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function library. Previous SMART robots, such as Herkamp’s Bigfoot, have utilized I2C 
communications, but the bi-directional nature of the signals coupled with I/O limitations 
of the BL2000 forced the researcher to divide the SCL and SDA lines into an input and 
an output pin for each signal [11], The BL2600 installed on the Creature was configured 
to use sinking current outputs, and achieves true bi-directional serial communications 
through two pins. 

C. PROGRAM FLOW 

Figures 34, 35, and 36 show flow charts of the wheel tachometer assembly 
language program. The main program element is a loop during which the PIC monitors 
three digital inputs. If a transition, a change in the digital input from a low to high logic 
state or high to low logic state, is detected, then the register associated with that motor 
shaft count is incremented. A short 40 ps delay is implemented after the three signals are 
tested for a transition in their logic states to allow for rise time in the digital signal lines. 
The PIC’s TIMER I counter is seeded with a literal value prior to executing the loop. 
During code execution, the TIMERl counter increments automatically from the seed 
value until the counter overflows, which occurs after 250 ms have elapsed. The overflow 
sets an interrupt flag. 

The remainder of the program, shown in Eigure 36, is an interrupt service routine 
(ISR) handler. Because the researcher lacked extensive assembly language programming 
experience, example code to handle the ISR tasks and I2C communications was found 
[38]. The example was modified to measure the wheel frequencies. The code monitors 
for ISR triggered by the overflow of the counter, which signals the completion of the 250 
ms timing. If the ISR was caused by the counter overflow, the counter’s overflow flag is 
reset along with the seed value for the counter for the next 250 ms period. To produce an 
output value in cycles per second from the number of transitions in 0.25 s, and assuming 
one cycle per two transitions, the code multiplies the number of transitions by two. The 
three values representing the frequency in cycles per second for the wheel rotational 
speed are saved to separate registers so as to be available when requested by the I2C bus 
master. 
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Initialization 
Configure PIC 



Figure 34. Tachometer Program Flowchart Page 1. 
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Figure 35. Tachometer Program Flowchart Page 2. 


D. DATA OUTPUT 

The ISR hander also parses ISRs triggered by I2C communications. A complete 
I2C bus communications description is beyond the scope of this thesis. One should 
consult Philips Semiconductor’s comprehensive document for more information [39]. 
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Figure 36. Tachometer Program Flowchart Page 3. 


Briefly, the BL2600 serves as the I2C bus master and controls the serial clock 
line, SCL. It initiates all communications at its leisure. To request wheel tachometer 
data from the PIC serving as the tachometer the BL2600 sends a start sequence, then the 
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tachometer’s seven-bit address, OxDO, with the LSB elear. The PIC’s built-in 
synehronous serial port (SSP) module detects the I2C start sequenee and verifies that the 
address matches the seven-bit address set in the eonfiguration portion of the eode. If the 
address matehes, the PIC’s serial port triggers an interrupt condition that can be parsed in 
the ISR hander. Next the BL2600 sends the byte of the register whose data it is 
requesting to read, and then resends the start sequence. Each event triggers an ISR and 
the ISR handler calls the serial port handler, SSP Handler. In the serial port handler, the 
serial port’s status flags are sequentially evaluated to determine which state of the I2C 
eommunications sequenee is oeeurring, i.e. start sequenee, write to PIC, read from PIC, 
ete. In State 3 of the serial port handler, the requested register byte sent from the BL2600 
is tested to determine whieh wheel speed value was requested. The appropriate register 
eorresponding to one of the three wheel speeds is saved to the PIC’s working register, 
WREG, in anticipation of the BL2600’s command to transmit data. In State 4 of the I2C 
communication sequence, the BL2600 sends the PIC’s seven-bit address with the ESB set 
to command the PIC to transmit its data. The PIC responds by calling the WriteI2C 
subroutine to write the value of the wheel speed in cycles per second, to the serial port’s 
buffer for transmit to the BE2600. 

The BE2600 Dynamic C code provides a feature-rich set of functions to 
communicate with I2C devices. Although the I2C specifieation provides for data rates to 
400 kbit/s, the Creature’s I2C communications were limited to a maximum 100 kbit/s due 
to the published maximum data rate by the Devantech digital compass [40]. Operation at 
100 kbit/s was not deemed possible due to the BE2600’s inability to provide for precise 
timing delays below 1 ms and for fear of introdueing errors due to missed bits. In plaee 
of timing delays using Dynamic C’s millisecond timer funetion, the BE2600’s I2C code 
was edited to use a short for loop to eount from zero to four between I2C cloek pulses. 

Observations of the SCE and SDA serial data transfers were made using an 
oseilloseope. The for loop delays the proeessor approximately 25 ps between I2C SCE 
clock pulses, which provides a 20 kbit/s data rate. A eomplete I2C sequenee to request 
one byte of data and read it in from a slave device, i.e., the Devantech CMPS03 magnetic 
compass, was observed to take 3.4 ms. This value is approximately three times longer 
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than predicted based on the assumed number of bits that must be clocked out by the 
master to communicate the data between the devices. The BL2600’s multithreading 
operation is the likely cause of the increased time required, since the processor cannot be 
guaranteed to execute code sequentially. 
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VI. ROBOT OPERATING PROGRAM 


The Creature’s operating program is written in Z-World’s proprietary Dynamie C 
9.21 programming language, whieh resembles the C language, and runs on a BL2600 
SBC. Like previous SMART program vehieles, the program was edited and eompiled 
using the assoeiated integrated development environment (IDE). The operating program 
is ineluded in Appendix F. 

A. CREATURE’S OPERATING PROGRAM REDESIGN MOTIVATION 

A number of robots preeeding the Creature have used the Dynamie C language as 
the basis for their operating programs. Miller employed an earlier Dynamie C version, 
7.04, with the BL2000 SBC [10]. Herkamp and Jun’s Bigfoot robot used the 
eombination of Dynamie C 9.21 and the Z-world BL2000 SBC with sueeess [11, 28]. 
The monolithie program used by Miller was an outgrowth of earlier work on the Bender 
robot, while Herkamp’s use of version 9.21 of Dynamie C represented a new line of 
operating program development [10, 11]. A eomplete deseription of the prior operating 
programs’ organizations is beyond the seope of this thesis. Prior operating programs 
displayed severe time lags during their program loops, though. Herkamp noted the 
Bigfoot operating program required 665 ms to eomplete its 12C eommunieations with the 
CMPS03 digital eompass, and Bigfoot oeeasionally failed to reaet in time to avoid 
eollision with obstaeles [11]. 

B. OPERATING PROGRAM IMPROVEMENTS 

The researeher deeided that a design goal for the Creature should be 
eomputational load distribution. In effeet, it was deeided to eliminate low-level tasks 
from the CPU’s robot operating program to free its proeessor for higher-level tasks to 
reduee the robot operating program’s loop eyele time. The sensor seetion deseribes how 
PIC mieroeontrollers were used towards this end. Computational load distribution was 
merely the first step in improving program exeeution speed. The Bigfoot Dynamie C 
eode was abandoned. Code from previous NPS roboties elass work by Le, Cole, and 

Gamble provided a starting point, instead [41]. Its TCP/IP funetions had been 
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successfully demonstrated in elass work, and showed promising improvements, namely 
redueed eommunieations lateney. It featured a modular, proeedural strueture with 
exeeution branehing off to eomplete a variety of tasks handled by disereet funetions. 

The Creature’s operating program substituted Dynamie C library funetions for 
I2C eommunieations. Ineffieient, CPU-intensive delays using loop eounters were 
eliminated. Expeeted delays between the CPU and peripheral deviees sueh as sensors, 
were aeeommodated with Dynamie C’s built-in multithreading eostatements. Code 
assoeiated with equipment that was not installed, sueh as a Global Positioning System 
(GPS) reeeiver, was deleted. Mueh new eode was ereated to interfaee with the newly 
ereated sonar sensor and three-wheel taehometer and to operate on the reeeived data. 
IMU eode was developed. Direetional eontrol feedbaek was added using the IMU 
rotational rate data about its Z axis. Navigation was approximated using a flat-Earth in 
the vieinity of the robot, and navigation from wheel odometry was added. 

C. OPERATING PROGRAM FLOW 

The program begins by deelaring variables whose seope is limited to the main 
funetion. Then the BE2600’s digital and analog inputs and outputs (I/O) are eonfigured 
followed by a funetion eall to initialize the robot, whieh effeetively sets various global 
variable default values and sets up serial eommunieations between the CPU and the sonar 
and IMU. The operating program then proeeeds to enter its main loop, whieh eomprises 
the sixteen eoneurrent eostatements shown in Eigure 37. Using the multithreaded nature 
of Dynamie C’s exeeution of eostates, the program employs shorter or longer time delays 
before exeeution is allowed to return to a partieular eostate. High priority tasks use a 
short time delay between eostatement revisits, while lower priority tasks give up 
exeeution for longer periods. 
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Figure 37. Flowchart of Robot Operating Program. 


1. Port Checking and Waypoint Costates 

The first two costatements in Figure 37 check for the presence of TCP/IP data 
packets on their ports. They are responsible for receiving the TCP/IP packets through 
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function calls to receive_packet_from_port. The Waypoint costate is triggered if 
waypoint data is reeeived by the above eostate. It saves the waypoints transmitted by the 
laptop’s Java graphieal user interfaee (GUI) and ealls funetions to eonvert and save the 
waypoint data into a Cartesian format. Next it initializes navigation and saves a waypoint 
array before starting the robot’s autonomous operation, setting its direetion to drive, 
setting the veloeity, and elearing the manual eontrol flag. 

2. I2C Compass and GUI Feedback Costates 

The program attempts to eommunieate with the I2C digital eompass and read in 
the magnetie heading. A feedbaek eostatement alternately sends the saved magnetie 
heading variable or robot position data to the GUI. Beeause the GUI expeets position to 
be reported in latitude and longitude, the robot’s Cartesian position is used to derive the 
reported position in the DR Costate. 

3. Sonar Ranging Costate 

The BL2600 does not trigger sonar ranging attempts, per se. Rather, the robot 
operating program uses the Sonar Ranging Costate to eonduet RS232 serial 
eommunieations with the Sonar Sensor Head Controller. The eostate begins by flushing 
any data present. Then it uses Dynamie C’s waitfor funetion to periodieally eheek for 
the presenee of serial data from the PIC serving as the sensor head eontroller 
mieroproeessor. When data is initially deteeted, the operating program allows the 
eomplete sonar range sentenee to be transmitted and then ealls the getSonarRanges 
funetion to read the data present in the serial buffer. In this manner, the CPU ean 
simultaneously eontinue exeeuting other eostates while the relatively slow peripheral 
eommunieations take plaee. The sonar ranges to objeets in the twenty azimuth bins 
around the robot are saved to a global array. Ranges are saved in eentimeters. 

4. Obstacle Avoidance Costate 

This eostate is aetive only if the operator has not taken manual eontrol of the 
robot. It tests the sonar range data in the global array above using the eheekSonarRanges 
funetion. IR range data, in the form of floating point values of the reported analog 
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voltages, are also eheeked using the irCloseContaet funetion. If the sonar funetion 
deteets an objeet within the CLOSERANGE limit, equal to 40 em, and it is within the 
azimuths in the robot’s direetion of travel, it sets the global variable eloseContaet, whieh 
triggers obstaele avoidanee. If the IR funetion returns a value indieating an obstaele 
along the 300“ azimuth, obstaele avoidanee will also be triggered. Obstaele avoidanee is 
a rudimentary random walk. The Creature slows, stops, and then rotates in plaee for a 
random period using the doRandSpin funetion. If the obstaele was deteeted with sonars, 
then sonar ranges are eheeked for objeets on either side of the 300“ azimuth. Similarly, if 
the obstaele was deteeted with IR sensors, the program assumes the sonar was not able to 
deteet the original obstaele, and IR ranges are used to test the 300“ azimuth for obstaeles. 
If the 300“ azimuth is elear, the robot moves away in that direetion. Otherwise it remains 
in the obstaele avoidanee loop and exeeutes another rotation in plaee. 

If no elose eontaets are deteeted, the Obstaele Avoidanee Costate tests the sonar 
data for the presenee of obstaeles ahead of it at ranges less than the MIDRANGE value, 
whieh is set to 90 em. If there are objeets ahead, the Creature is slowed, but not stopped. 
The eostate resets the veloeity to the default autonomous speed if no sonar eontaets ahead 
of the robot are deteeted inside the MIDRANGE limit. 

5. IR Ranging and Wheel Tachometer Costates 

The CPU samples the analog IR ranger voltages with a funetion eall to getIRVolts 
within the IR Ranging Costate. The CPU eonduets I2C eommunieations with the PIC 
serving as the wheel taehometer to read in the observed wheel speeds using the wheel 
funetion. The funetion uses I2C library funetions to request the value, in Hz, of eaeh of 
the three wheel speeds. The observed wheel speeds are stored to global variables in 
radians per seeond. Based on the wheel speeds reported by the taehometers, the CPU 
ealeulates the direetion and magnitude of the robot’s veloeity veetor in the robot frame of 
referenee. It also finds the amount of ehassis rotation, D, and sets the global newDR flag 
to indieate the presenee of new data upon whieh to dead reekon the robot position. 
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6. Dead Reckoning (DR) and NavigationCostates 

The odometry data, in the form of wheel speeds, must be eonverted into the frame 
of referenee of the plane that the robot is operating on. This Earth frame of referenee is 
assumed to be a stationary plane, and the DR Costate ealls the ealeVeloEarthFrame to 
eonvert the robot veloeity veetor from the robot frame to the Earth frame using the 
magnetie heading to relate the two frames of referenee. The funetion ealled dr (short for 
dead reekon) estimates the robot’s Cartesian position in eentimeters from the Origin, 
whieh is set to the loeation of the robot when the waypoint data was transmitted. It 
assumes straight-line motion between eaeh update using the veloeity ealeulated in the 
Earth frame. The Navigation eostate is responsible for periodieally reealeulating the 
navigation solution, the eommand heading, required for the robot to drive from its DR 
position in the Earth frame to the next waypoint. It uses a flat Earth approximation and 
solves the aretangent to determine the direetion needed to drive to the waypoint. 

7. Position Formatting Costate 

This eostate simply ealls a funetion, makeFakeGPSpos that operates on the 
Cartesian DR position to produee a latitude and longitude. It ereates a string resembling 
a GPS NMEA sentenee. 

8. IMU and Heading Hold Costates 

The program’s IMU Costate tests for the presenee of serial data on the RS232 port 
eonneeted to the IMU. If the test does not fail to indieate the presenee of data, the eostate 
tests if the data indieates the start flag of an IMU data sentenee. It uses the waitfor 
funetion to allow program exeeution to eontinue briefly while allowing the IMU to send 
its eomplete sentenee before reading in the aeeeleration and rotation rate data with the 
getimu funetion. The Heading Hold Costate operates if the global holdHeading flag is 
set and the robot is not stopped. The eostate applies proportional and derivative eontrol 
feedbaek to maintain heading. The eostate delays operation again until the reeeipt of new 
IMU data. 
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9. 


Manual Control Costate 


This costate allows the operator to remotely control the robot. If manual control 
data packets are present, the global flag, manual_control_flag is set. This overrides 
autonomous operation in various costates. The costate calls the manual control function, 
which parses the manual control string received from the GUI and sets the desired 
direction of motion and magnitude of the velocity vector. The function calls the vector 
function to generate the desired motion. 

10. Stuck Vehicle Watchdog Costate 

A low-priority costate is used at the end of the main loop to test for a condition 
that indicates the robot is stuek. It tests for a condition where the robot is sending the 
motors non-zero speed signals, i.e., it is attempting to move, but the three observed wheel 
speeds, from the tachometer, are nearly zero. The tachometer allows the costate to 
provide a simple feedback mechanism for detecting gross speed errors. If the condition is 
detected and it persists for a period equal to TIMELIM, equal to four seconds, the robot 
initiates action to attempt to free itself 
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VII. PULSE WIDTH MODULATION (PWM) CIRCUIT 


A. EXPERIMENTAL DESIGN 

Experimental observations were made to determine the linearity of the PWM 
eireuit’s response to the analog speed signal by measuring the voltage supplied to eaeh 
motor. The eireuit was designed to produee a 100% duty eyele PWM signal at 4.1V. 
This voltage eorresponds to the maximum analog voltage that the BL2000 single board 
eomputer ean supply. To faeilitate the test instrumentation the robot needed to remain 
stationary during the test, so the robot was plaeed on wooden bloeks that prevented its 
wheels from eontaeting the ground. As a result all the observations of the assoeiated 
motor voltages, PWM duty eyeles, and shaft speeds refleet a no load eondition. 

A Fluke multimeter was eonneeted to measure eaeh of the three motor eontroller’s 
output voltage. Eaeh multimeter’s ground referenee was eonneeted to the motor 
eontroller’s terminal labeled Ml, and the positive lead to the M2 terminal. Another 
multimeter was used to measure the DC analog speed signal supplied to the PWM eireuit 
board. The DC power supply’s ground referenee was eonneeted to the eleetronies power 
supply ground. The installed eleetronies battery aboard the robot was used to provide 
eleetrieal power to the CPU, and I2C deviees via the five-volt bus. 

The two motor batteries were eonneeted in series and their series voltage was 
applied to the motor power bus. A mulitmeter was used to reeord the voltage potential 
between the motor bus’ unregulated voltage test point on the distribution/test panel and 
the motor ground test point on the same panel. This measurement was done with the 
analog speed signal set to zero at the beginning and end of the sequentially inereasing test 
deseribed below. 

Analog input DC voltages were applied beginning at 0.0 V and were sequentially 
stepped up in 0.2 V inerements starting at 1.0 V. The analog speed signal was eonneeted 
in parallel to the three motor speed input pins on the PCB. Thus, the eireuit reeeived the 
same analog voltage input for all three motors, and the motors were operated 
simultaneously. The robot does not utilize the motor eontrotier’s brake, so the eireuit on 


91 



the PCB holds this signal low, and the direetion signal was allowed to remain low. The 
motor eontroller output voltage applied to eaeh of the three motors was reeorded for each 
analog input voltage. Extra measurements were taken near the upper limit of 4.1 V. 
After the data were recorded for the 4.1 V input voltage, the input voltage was reset to 
zero, and the motor battery voltage observed as described above. 

The motor controller output voltage test described previously included added 
variability since it depended on the response of the LMD18200 H-Bridge chip, the 
resistor, and capacitors that were installed on each motor controller. The PWM 
waveform results from the intersection of the PWM circuit’s sawtooth signal and the 
analog speed signal. In turn, the PWM signal is interpreted by the motor controllers, 
which regulate the motor voltage applied to the motors. Thus, the PWM signal was not 
anticipated to depend on the motor battery voltage, and this voltage was not measured in 
this experiment. A follow-on experiment was conducted to measure the PWM circuit’s 
response independent of the motor controller. This experiment consisted of applying an 
analog speed signal to the PCB’s input and observing the PWM waveforms and their duty 
cycles that were produced by the PWM circuit. 

The PWM output signals were disconnected from the motor controllers’ inputs. 
A Tektronix TDS3034 oscilloscope was used to measure the duty cycle of the three 
PWM signals. The scope probes were connected to the PWM printed circuit board 
(PCB) speed signal output pins and grounded to the motor battery ground test point at the 
distribution/test panel. The Tektronix oscilloscope’s built-in percentage high duty cycle 
measurement was used. A multimeter was used to measure the analog input speed signal 
as in the above test. 

As above, the analog speed input signal was connected in parallel to the three 
PWM PCB input pins. The same analog DC voltage was applied to the PCB’s three 
speed inputs starting at 0.0 V. Beginning at 1.2 V the input was sequentially stepped up 
in 0.1 V increments to 4.1 V. For each analog input speed signal, the percent high duty 
cycle measured by the Tektronix oscilloscope was recorded. 
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B. PWM CIRCUIT EXPERIMENTAL OBSERVATIONS AND ANALYSIS 


Data gathered in the experiments were interpreted using MATLAB. Since the 
motor batteries’ voltage will decay during operation, the voltages applied to the motors 
were normalized to allow more meaningful interpretation. The starting and ending motor 
battery voltages were averaged. Figure 38 below shows the ratio of the DC motor 
voltage output to the average motor bus voltage as a function of analog speed signal 
applied to the PWM circuit. 
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Figure 38. DC Motor Voltage output as a Function of Analog Input Voltage. 
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Below 1.2 V the DC motor voltage was observed to be zero. The data below 1.2 
V were omitted to faeilitate a linear fit. A first degree fit of the normalized DC motor 
voltage yielded the following relation to the analog speed signal. Vout is the measured 
output voltage, and Vavg battery is the average battery voltage, i.e., the maximum value 
attainable. Vspeed is the independent variable, the analog speed signal applied. Of note, 
the motor voltages observed from the number one motor controller were consistently 0.07 
to 0.06 V higher than those observed from numbers two and three. 




V. 


0.330(v.„„)-0.379 


avg .battery 


Oscilloscope data are interpreted below. Figure 39 shows the duty cycle of the 
PWM waveforms as a function of the analog speed signal. The figure includes data 
below 1.2 V, and one can see the duty cycle goes to zero in this region. 
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Figure 39. PWM Duty Cycle as a Function of Analog Input Voltage. 
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The PWM duty cycle data showed that the number three signal was consistently 
0.4 to 0.6% higher than the numbers one and two signals. Figure 40 illustrates that the 
standard deviation of the three duty cycles were acceptably small but increased with 
increasing analog input voltage. Considering the observation above, there is no evidence 
that the PWM signals were responsible for the increased output voltage of the number 
one motor controller relative to the other two. Rather, the researcher suspects the number 
three motor controller produced the discrepancy. 
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VIIL SONAR SENSOR HEAD 


Because of the robot’s holonomic motion, such motion could result in a collision 
with an obstacle while translating in any direction. To prevent this, a sensor was 
developed that could provide coverage of all azimuths around the robot. Additionally, 
complete azimuthal coverage was necessary to implement Virtual Force Field (VFF) or 
Vector Field Histogram (VFH) obstacle avoidance methods used by Borenstein [42, 43]. 
Ultrasonic sonar was chosen because it was inexpensive, tolerant to timing errors, 
accurate in range, and proven over decades of robotic research. 

Complete azimuthal coverage is inversely related to the beamwidth; wider beam 
patterns provide coverage using fewer transducers or fewer sonar ranging attempts. 
Wider beamwidths, however, produce imprecise azimuthal detections. Narrower 
beamwidths, conversely, require more sonar ranging attempts. Increasing the number of 
ranging attempts slows the update rate of the sensor system but provides more fine¬ 
grained azimuthal information on detected objects. The electrostatic Polaroid transducer, 
with its nominal 15° beamwidth, was chosen for the Creature. When paired with the 
Senscomp ranging module, it provides a simple digital sonar solution and offers a good 
compromise of directionality and coverage. 

In the past, robots such as CARMEL have addressed the problem of complete 
azimuthal coverage with a fixed installation of 16 to 18 sonars distributed at regular 
angular intervals about the periphery of the robot [42]. This solution was simple and 
required no moving parts, but its cost, complexity of installation, weight, etc were not 
optimal. The Creature’s rotating sonar sensor head combined the desirable aspects of the 
Polaroid transducer’s beam pattern, but did so with four orthogonally mounted 
transducers. In place of 16 fixed devices, the sensor head achieves complete coverage by 
mechanically scanning the sonar transducers in azimuth. The sensor head consists of an 
RC servo motor, a square perf board mounting plate, and four transducers. 
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A. SONAR RANGING PATTERN 

Figure 41 shows the scan pattern of the sensor head relative to the robot’s X,Y 
reference frame. Sonar number one is oriented 60“ from the robot’s Y axis when at 
position three. Note, times reflect the position of the sonar detector head after it has 
reached the position but before firing the sonars. The times are based on optimized 
delays when operated with the final, adjusted version of the microcontroller’s code. 

The 360“ surrounding the robot were divided evenly into twenty, 18“ sectors by 
positioning the servo sequentially to five azimuths and ranging the four orthogonally- 
mounted sonar transducers once at each position. After ranging the sonars at one position 
the servo was moved counter clockwise and the process repeated. After the four sonars 
were fired at position number five, the servo moves clockwise to position number one 
and the cycle begins again. 
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Figure 41. Sonar Sensor Flead Scanning Sequence. 
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The RC servomotor was eontrolled by the sonar sensor head eontroller, whieh 
eontains a PIC16F690 mieroeontroller. In all experiments, the PIC assembly eode 
delayed 33 ps while listening for the return eeho plus 20 ps to eliminate mutipath 
deteetions between eaeh ranging attempt. The delay between sonar firings ineluded an 
additional 80 ps when moving the servo motor. The expeeted time to eomplete ranging 
20 seetors was 1460 ps. The sonar sensor head eontroller was responsible for the 
following aetions; 

• generating the servo positioning signal 

• triggering the sonar firings 

• finding the TOP of the pulses 

• storing the TOP data 

• downeonverting the 16-bit TOP data to integer, one byte format 

• eommunieating the one byte data via RS-232 to the robot’s CPU 

B. SONAR SENSOR HEAD EXPERIMENTAL DESIGN 

A number of experiments were eondueted to measure the performanee of the 
sonar deteetor head. The experiments deseribed below were designed to find the 
following oharaeteristies; 

• operation of the four transdueers 

• eoeffioient to eonvert the sensor’s one byte data to eentimeters 

• range resolution 

• minimum deteetable range 

• maximum deteetable range 

• angular resolution of the servo seanning mechani s m 

• cycle time required to range all sectors 

The Polaroid transducer’s acoustic beam pattern had been studied and well 
documented. No effort was made to duplicate this work; rather observations were made 
of the PIC microcontroller’s output data and the mechanical operation of the sensor head 
with the servo motor operating. 
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All of the experiments deseribed below were eondueted in the Naval Postgraduate 
Sehool’s aeoustie anechoie room. The room is designed for aeoustie frequeneies below 
the 49 kHz ultrasonie frequeney of the Polaroid transducer. It was immediately observed 
that the room’s walls reflected sufficient acoustic energy to register as valid echoes even 
in the absence of the test targets. No objects, aside from test targets, were allowed within 
two meters of the center of the robot. Walls were 2.2 to 3.5 m from the sonar 
transducers. Test targets were 0 0.5 inch solid steel rods, roughly 2 feet long. The 
targets were positioned vertically. The air temperature during the experiments was 20.5 
“C. 

During the experiments the robot remained stationary, and an AC to DC power 
supply was used to supply DC voltage to the electronics power bus in place of the 
electronics battery. The first-generation five-volt bus was energized to power the sonar 
sensor head controller. The five-volt bus’ servo switch was energized when needed for 
experiments that required scanning the sonar transducers in azimuth. Stationary sonar 
sensor head observations were done by leaving the servo un-powered. The motor power 
bus remained off, and motors were not operated. The robot’s BL2600 CPU and I2C bus 
power remained off. 

In place of the CPU, a laptop computer with an RS-232 serial port was connected 
via serial cable to the serial port on the sonar sensor head controller PCB. The 
Hyperterminal application bundled with the laptop’s Windows OS was used to observe 
the serial data transmissions. The researcher observed the ASCII characters transmitted 
from the PIC microcontroller and then translated them into deicmal integer values to 
interpret the TOP. 

The data were one-byte values; hence one would expect valid data over the range 
zero to 255. The PIC, though, is programmed to send the hexadecimal value 0x02 for 
any received echo with integer TOP value greater than 250. This allows it to use the 
ASCII characters from 251 to 255 as control flags. Por example, ASCII 254 and ASCII 
251 indicate the start and stop, respectively, of its data word. Sonar blanking of the 
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transmitted pulse governs the minimum deteetion range, whieh, in turn, limits the 
expeeted minimum one-byte value. The minimum one-byte value was expeeted to be 
approximately 19. 

The first observation made was a eontrol with the sensor head in a statie eondition 
and no targets present. No objeets were observed within two meters of the robot. The 
sonar sensor head’s TOP data byte was reeorded. 

Next an experiment was eondueted to verify the operation of the four transdueers 
and to determine the linear eoeffieient needed to eonvert the one-byte TOP value to a 
range in eentimeters. The test was statie, the servo left unpowered. A target was plaeed 
at 100 em distanee in the eenter of eaeh transdueer’s beam pattern, and the sonar sensor 
head eontroller’s output was observed. After observing the reported TOP data byte, the 
distanees to the four targets were verified. Two targets required small distanee 
adjustments, as they were not preeisely plaeed. The targets were adjusted and the TOP 
range bytes were reeorded. 

A brief test was done to determine range resolution, hollowing the operational 
test above, two targets were moved to 98 em, while two were left in their original 
positions as eontrols. The range data bytes were reeorded for the four targets. 

A minimum range experiment was eondueted to determine the eoeffieient 
deseribed above and the minimum deteetable range. The four targets were removed and 
the sensor head was reoriented manually before the minimum range test was done. With 
the sensor head stationary, a target was plaeed in the eenter of one of the transdueer’s 
beam patterns at 50 em. The distanee was deereased to 30 em, 24 em, and then 20 em 
and the TOP data were reeorded at eaeh distanee. At 20 em the target’s tripod legs 
eontaeted the robot’s wheel and prevented plaeement eloser than 20 em. 

A maximum range test was done in mueh the same manner as the minimum range 
test. The first experiment was attempted with a target at two meters and within about 30 
em of the eomer of the room, but the proximity of the room’s eorner to the target 
prompted the researeher to abort the test and plaee the target in the eenter of the aneehoie 
room. Beginning at 2.0 m, a target was plaeed in the eenter of the same transdueer used 
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for the minimum range test. At eaeh distanee, the range data byte was reeorded. The 
target was repositioned to 2.2 m, 2.3 m, and 2.4 m. At the eompletion of the maximum 
range test, the target was removed, and the range byte reeorded with no target present. 

Two experiments were eondueted to assess the meehanieal seanning of the sensor 
head. Unlike previous experiments, the sensor head was allowed to move to the five 
positions deseribed above by powering the RC servomotor. In the first, targets were 
plaeed in the eenters of three adjaeent sonar ranging seetors, at 100 em range. Measuring 
from the robot’s Y axis, whieh eoineides with the number one motor shaft, a target was 
plaeed at 24°, 42°, and 60°. These positions eorrespond to the predieted eenters of three 
seetors, or azimuth “bins.” After observing the reported range data three times, the eenter 
target at 42“ was removed and the range data were reeorded another three times. The test 
was repeated with three targets plaeed -30“, -12“, and 6“ relative to the robot’s Y axis at 
100 em. 

The last azimuth seanning experiment eonsisted of plaeing one target at 1 m range 
and varying its azimuthal position to determine whieh seetor would register the eontaet. 
The target was initially plaeed at -30“ relative to the Y axis. Beeause the sonar’s beam 
pattern is approximately 15“, moving the target 7.5“ off the eenterline of the seetor was 
expeeted to produee no eontaet. The target was repositioned to -37“, -39“, and then to 
-43“ relative to the Y axis while maintaining range eonstant at Im. The range data were 
reeorded for eaeh seetor in the vieinity of that expeeted to register the target’s presenee. 
Analysis of the seetor edge experiment prompted the researeher to eonduet it a seeond 
time. In the seeond experiment, the target was initially plaeed at 1 m along the -30“ 
azimuth. It was moved eounter eloekwise to -39“, -43“, and -47“ positions. Then it was 
plaeed at -23“ and moved to -12“ and 0“. 

A simple experiment was eondueted to verify the time required for the seanning 
deteetor head to eomplete a eyele of 20 sonar-ranging measurements. The time for the 
sensor head to return to its initial position was measured and averaged. 
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C. SONAR SENSOR HEAD EXPERIMENTAL OBSERVATIONS 

The stationary control yielded surprising results. The acoustic anechoic room was 
expected to offer a pristine environment in which only echoes from the test targets would 
be detected by the Senscomp sonar modules. During the control, however, the sonar 
sensor head controller reported the one-byte decimal values 174, 173, 185, and 198. 
These likely corresponded to echoes detected from the room’s walls. 

The 1-meter static test with four targets followed. Three sonars reported 95, and 
one sonar reported 94. The range to the number one target, the target closest to the Y 
axis, was measured at 101.5 cm, and it was repositioned to 100 cm. The test was 
repeated with three sonars reporting 94 and one reporting 95. The range to the number 
four sonar target was re-measured at 100.5 cm. After repositioning this target to 100 cm, 
the reported ranges were 94 for all sonars. This reported range data was constant for the 
duration of the observation, approximately 30 s. 

Targets number one and four were moved to 98 cm. The observed range data 
bytes were 93 for targets one and four, and 94 for targets two and three. Observed range 
resolution in this test was 2 cm. Expected resolution is limited by the process in the 
assembly code that downconverts the sonar TOE using the PIC’s sixteen bit count of its 
TIMERl oscillator to an eight-bit value. Operating at 8 MHz, the PIC’s TIMERI 
oscillator has a period of 0.5 ps. The downconverting process discards the most 
significant bit, 215, and the least significant bits 20 through 26. The product of 27 and 
0.5 ps, the oscillator’s period, yields a 64.0 ps period for the 27 bit, which one can 
multiply times the speed of sound in air to obtain 2.17 cm per 27 period. The product of 
TOP and speed of sound must be halved to give range, which yields 1.09 cm per 27 
period. The least significant bit (ESB) reported by the PIC is the 27 bit, so the ESB 
transmitted via RS-232 in the output data represents roughly 1 cm range. 
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Observed range data bytes agreed with the expeeted results. The table below 
summarizes the observations from the tests above and the minimum range test. In the 
minimum range test, valid ranges were reported to the design limit of 20 em, whieh 
roughly eorresponds to an objeet in eontaet with the perimeter of the robot’s eireular 
ehassis. The reported range data byte did not vary during any of the observation periods. 


Target Range 

Data byte reported by mieroeontroller 

20 em 

21io 

24 em 

2510 

30 em 

30io 

50 em 

oo 

o 

98 em 

9310 

100 em 

O 

o^ 


Table 4. Reported Range Byte Value vs. Measured Range. 


The ratio of target range in eentimeters to the one-byte range value is proportional 
to range. If one desired to optimize the sonar deteetor for preeision at a partieular range, 
one eould seleet the ratio eorresponding to that operating distanee. Additionally, this 
eoeffieient will vary with the speed of sound in air; thus it is inversely proportional to the 
air temperature. This researeher has averaged the values from 100 em to 50 em to 
provide the operating program the needed eoeffieient to eonvert the reported data byte to 
eentimeters. 

Measuring the range to the targets in the maximum range test was less preeise 
than above, so it is not reeommended that one inelude the data in determining the 
multiplieative eonstant. The observed data at 2.0 m and 2.2 m were 185 and 198, 
respeetively. Multiplying by the eoeffieient derived as deseribed above gives 196 em and 
210 em. The observed data byte with the target at 2.3 m and 2.4 m varied around 209 and 
210 and was indistinguishable from the eontrol with the target removed. It is likely the 
sonar transdueer was deteeting returns from the wire mesh floor of the room. 
Multiplying the nominal height of the sonar transdueer, roughly 25 em, times the tangent 
of the half-angle beam width, 7.5°, indieates one might expeet the sonar to deteet the 
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floor at about 1.9 m. While in the aneehoie room, the robot was plaeed on a 2x4 wood 
bloek. The sonar transdueers were roughly 3 em higher than if the robot had been plaeed 
on its wheels atop a flat surfaee, and repeating the ealeulation prediets a possible eeho 
from the ground at 2.1 m, whieh agrees loosely with the observations. 

The results of the azimuthal sean tests agreed with expeetations. The test, whieh 
involved plaeing a target in the eenter of three adjaeent seetors, was eondueted twiee. 
Figure 42 shows the graph of observed range as a funetion of the seetor number. The X 
axis is the numbered seetor, whieh ranges from zero to nineteen. Seetor zero eorresponds 
to the eighteen-degree seetor whose eenter is aligned with an angle six degrees from the 
robot’s Y axis. Eaeh sequential seetor is eentered eighteen degrees to the right, or in the 
eloekwise direetion as viewed from above the eenter of the robot. In the lower portion of 
Figure 42, the two graphs inelude seetors on both sides of the robot’s Y axis, so the 
notation -1 has been used for the nineteenth seetor and -2 for the eighteenth. 

The average of the observed range byte values is plotted for the five seetors. The 
eenter three seetors indieated targets at the expeeted range. In the first eolumn, one ean 
see the three targets and the baekground walls in the seetors on the edges. The right 
eolumn shows range as a funetion of seetor with the eenter target removed. The resulting 
graph shows two eontaets at ranges approximately Im, but the now-empty seetor shows 
the baekground range to the room’s wall. 
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Figure 42. Detection of Targets in Three Adjacent Sectors. 


Another azimuth scanning experiment was conducted to test how the sonar sensor 
head controller reported targets on the fringes of adjacent sectors. A single target was 
placed at -30“ from the robot’s Y axis at Im. It was expected that this position would 
coincide with the center of the eighteenth sector. At -30“ the target was registered by the 
sonar detector controller and reported in the eighteenth sector as expected. Moving 
counter clockwise to -37“, seven degrees off the axis of the sector, had no effect on the 
range data byte. At -39“, the target was also reported in the eighteenth sector. At -43“ the 
target was reported in both the seventeenth and eighteenth sectors. Thus, although the 
3dB down point on the transducer’s beam pattern produces a nominal 15“ beam width, 
the sound intensity returned from the target to the Senscomp ranging module by the 
transducer’s side-lobes exceeds the minimum needed to register as a valid contact. The 
effect is an overlap between sectors for this combination of target and range. 
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The experiment was repeated and expanded to inelude attempts to find the limit of 
the seetor on the edge elosest to the Y axis. At -30° the target was reported in the 
eighteenth seetor, as expeeted, and at -43° targets were reported in the seventeenth and 
eighteenth seetors. At -47° the target was reported in the seventeenth seetor. Moving the 
target eloekwise from -47° to -23° generated eontaets in the eighteenth and nineteenth 
seetors. At -12°, the target was reported in the nineteenth seetor only. When the target 
was plaeed along the Y axis, the reported eontaet eorresponded to the baekground wall. 
Effeetively, the target was undeteeted when plaeed at 0°. 

Based on the observations of the behavior with respeet to targets near the Y axis, 
it was eoneluded that the PIC’s assembly eode was providing an insufficient time delay, 
80 ps, for the servo to move the 72° from its final position to the initial position before 
the sonar was fired. 

The time to complete a full 360° scan was measured. The observed time, 1.7 s, 
agreed with the expected 1460 ps. The PIC’s code was edited to remove the 20 ps delay 
for multipath avoidance between sonar firings. This could allow unwanted multipath 
detection, but the 33 ps listening period was deemed long enough to mitigate this. The 
servo delay between positions five and one was increased to 160 ps from 80 ps. Other 
servo delays were set to 60 ps. An abbreviated experiment conducted in the autonomous 
vehicle lab revealed a target placed on the Y axis was detectable with the modified 
delays. 

Figure 43 shows the results of one complete scan conducted inside the Physics 
Department’s ground floor hallway. Ranges are raw, one byte values reported and have 
not been converted to centimeters. The robot was stationary and placed midway between 
each wall with its Y axis normal to the walls. The long-axis of the hallway was oriented 
along the X axis. The linear features of the walls are easily visible. Note, the sonar 
sensor controller reports non-contact with the value 250, so one can see that no contacts 
were detected along the positive X axis. 
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Figure 43. Sonar Azimuth Scan Showing Linear Features of Hallway. 
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IX. FUTURE WORK 


Although the present robot’s eonfiguration represents many months of work, 
mueh work remains to be done to eomplete a prototype that addresses the original 
mission to deteet FOD and remove it from the robot’s operating environment. Some of 
the outstanding ehallenges represent traetable, albeit diffieult, problems that exeeeded the 
researeher’s ability and knowledge as well as the time eonstraints assoeiated with the 
projeet. Areas of future researeh ean be broadly grouped into five areas: FOD deteetion, 
FOD removal, operating program improvements, GUI/human interfaee, and navigation. 

A. FOD DETECTION 

An autonomous robot represents a eomplex deviee, but developing a sensor 
eapable of deteeting small targets of a variety of materials from plastie to non-ferrous 
metals, in an environment with poor illumination presents future researehers with a 
serious ehallenge. A variety of methods were eonsidered in the eoneeptual design phase 
of the Creature’s development. Ultrasonie sensors were briefly eonsidered, but it was 
believed their wavelengths would be too long. Ultimately, it was determined that visual 
wavelengths would likely be best for eommon FOD targets, sueh as washers, rivets, 
safety wire, ete. with eharaeteristie dimensions on the order of a millimeter. 

A eoneeptual sensor system was devised based on visual flow teehnology 
available with MATLAB’s Simulink. An example Simulink Video and Image Proeessing 
Bloekset demonstration is presently available that deteets moving objeets, automobiles, 
against a fixed baekground, eonsisting of a highway viewed from a stationary eamera 
mounted on an overpass above the roadway [44]. In the eoneeptual system for the 
Creature, a fixed eamera on the robot would stare at a swath of the surfaee along the 
direetion of the robot’s travel. The fixed eamera’s video eould be linked wirelessly to an 
operator’s laptop or workstation running the eomputationally intensive video image 
proeessing bloekset in Simulink. The expeeted operating environment’s surfaee would 
eonsist largely of steel deeks eovered by gray non-skid eoating, so a eolor fdter might be 
applied to the video stream from the fixed eamera to filter out the baekground deek and 
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render objects on top of the surface more distinguishable. An optical flow Simulink 
block in the blockset might be applied to the filtered image to detect the objects as their 
apparent motion causes them to move through the camera’s field of view, much like the 
automobiles move against the fixed background in the example above. 

B. FOB REMOVAL 

As a prototype, the Creature was intended to be a proof-of-concept device. The 
robot was intended to explore holonomic motion, navigation, and obstacle detection and 
avoidance. No effort was made to include a payload in the robot. Rather, it was expected 
that the Creature could demonstrate the feasibility of autonomous FOD detection. A 
follow-on robot could be built with smaller commercially-made devices, shrinking the 
size of many of the required components and freeing up space and weight for a payload. 
An engineering research area in the future might be design of a payload capable of 
removing FOD after it is detected. Vacuum and brush techniques might be explored as 
well as a method for holding the recovered FOD. Because vacuums and brush motors 
will almost certainly contain at least one high-current motor, efficient use of available 
battery power should be emphasized. Research must be done on integrating such a 
payload, with its high-current device, into the robot’s electrical power supply busses. 

C. OPERATING PROGRAM IMPROVEMENTS 

There are two aspects of the robot operating program that require additional work. 
First, the current robot operating program is brittle with respect to peripheral device I/O. 
Second, the Creature has only rudimentary obstacle avoidance algorithms, and it lacks 
path planning. 

Little provision has been made to detect if sensors are operating correctly, or if 
they are installed and receiving power. Portions of the Dynamic C code assume the serial 
communications will occur, and if they do not, the code can become indefinitely stuck 
awaiting the expected communication. The functions responsible for RS232 
communications between the BL2600 and the sonars and between the BL2600 and the 
IMU need a form of watchdog time out. For example, if the expected communication 
with the sonar sensor head controller is not completed within roughly 60 ms, then the 
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function might return a value to the main function to indicate a failure. The costate 
responsible for eommunieating with the sonars should be modified to note the failure and 
modify its exeeution. If a suffieient number of eonseeutive failures due to laek of reeeipt 
within the allowed listening time oeeur, then the robot operating program might be 
modified to assume a sonar fault exists, stop wasting CPU cycles attempting to conduct 
communications that are likely to fail, and implement an IR-only obstacle avoidance 
behavior. Communications with I2C devices use the I2C library functions, and these 
have been configured in the library to limit the number of retries to prevent the CPU from 
spending exeessively large numbers of processor cyeles attempting to communieate with 
deviees that are not responding. Regardless, a diagnostie costate might be added to eheek 
for the proper funetioning of all peripheral deviees on the I2C bus as well as proper 
operation of devices eommunieating via RS232. 

Obstacle avoidanee and path planning should be improved. The researcher was 
only able to develop a random walk behavior in the time available. Researeh was done 
into potential field algorithms, partieularly Borenstein’s virtual force field (VFF) and 
veetor field histogram (VFH), for implementation into the robot operating program as an 
underlying obstaele avoidance algorithm to eombine ultrasonie sonar and IR sensor range 
data. Borenstein’s work using ultrasonic sensors identical to those on the Creature 
allowed motion at up to 0.78 m/s while successfully avoiding obstacles [42, 43]. A large 
portion of the work to implement VFF was done, but not completed or tested. Dynamic 
C code was written to map the sonar range and azimuth data to a Cartesian frame of 
reference with its origin at the center of the robot, and functions were written to update 
the Cartesian representation of the sonar data for straight-line relative motion between the 
objeets and the robot. No code exists yet to handle rotational transformations required 
when the robot rotates with respeet to the Earth frame of referenee. Also, a function 
would need to be written to sum the repulsive forees and target’s attraetive foree to 
produce the resultant force on the robot. 
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Without adequate orientation data about the robot’s pose, the existing eode eannot 
aeeurately determine the position of the robot from wheel odometry. This problem 
relates to the separate navigation problem below. Thus, without aeeurate orientation, 
navigation aeouraey is poor, and as a result, path planning is moot. The researeher 
believed it was pointless to attempt to navigate from an erroneous position to a waypoint 
position, so path planning was strieken from the robot operating program. A random 
walk was implementated, instead. If navigation ean be improved in the future, path 
planning should be re-introdueed into the operating program eode to extend the 
Creature’s ability to eonduet other tasks. Future researeh eould simulate the motion of an 
omnidireetional robot using a random walk to seareh an area. Simulations eould eompare 
area eovered by random motion to area eovered using a planned seareh. Future researeh 
eould determine if random searehes for FOD are suffieient to solve the problem of 
deteetion and removal. If so, eost savings eould be realized, and a simplier follow-on 
robot eonstrueted. 

D. GUI/HUMAN INTERFACE 

The Java language GUI provides a useful means to monitor and eontrol the 
Creature, but its baekground as a front-end for eontrol of robots in an outdoor 
environment has hobbled it when the researeher modified it for use indoors with the 
Creature. Fundamentally, the GUI expeets the robot to use GPS for absolute positioning. 
Clearly this is a bad assumption when dealing with robots operating indoors, inside the 
interior of a ship, underground, or in any other environment where GPS is unavailable. A 
work-around was to take the Creature’s dead reekoned position, whieh was stored as a 
Cartesian position from the origin, a known starting point in the Earth frame, and 
periodieally eonvert the orthogonal East and North distanees from the known point into a 
latitude and longitude. The GUI eould be modified in the future to aeeept either 
latitude/longitude or Cartesian position information. 

Manual eontrol of the Creature with the existing GUI is adequate, but slow 
eompared to the robot’s eapabilities. Speoifieally, the ability of the operator to interpret 
the robot’s pose and send the eorreet driving eommands limits the speed at whieh the 


112 



robot can move through its environment. As a holonomie platform, the Creature has no 
front or rear. The laek of sueh arbitrary direetions means an operator eontrolling the 
Creature has many ehoiees when trying to move the robot. Additionally, the robot 
provides poor visual eues as to its orientation when viewed at distanees exeeeding three 
to four meters. A valuable area of future work would be the ereation of an improved user 
interfaee, eomplete with joystiek eontroller, to veetor the robot and a jog-shuttle to 
eommand robot ehassis rotation. Additionally, future researehers might implement an 
improved display, possibly a three-dimensional one fixed to the Earth referenee frame, to 
allow the operator to easily interpret the robot’s pose and quiekly apply the desired 
driving eommands. 

E. NAVIGATION 

The problem of determining robot position without GPS was diffieult to solve. 
Previous SMART program robots have assumed GPS would be available and depended 
on it for the primary souree of position information. In an environment without GPS, 
researehers are left with traditional methods sueh as dead reekoning, e.g., from wheel 
odometry, and beaeon methods. A vibrant and exeiting area of researeh eould be the 
implementation of a newer navigation method based on robotie vision. The researeher 
believes the eomputational load to derive robot motion from a video stream would vastly 
exeeed the limits of the BL2600’s proeessor, so a remote eamera method sueh as the one 
demonstrated by Shimada et al. might be employed [45]. In this position eorreetion 
method, a fixed remote overhead eamera observes a seene in whieh the robot operates. 
From the video image’s pixel data, a remote eomputer running a vision algorithm 
determines the robot’s position and orientation. The position is eommunieated to the 
robot periodieally. Between updates, the robot uses dead reekoning teehniques to 
maintain its position. 
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APPENDIX A - PULSE WIDTH MODULATOR AND 
OPTICALISOLATION CIRCUIT 


Figures 44 and 45 show the schematic of the PWM and optical isolation circuit 
that was constructed to send analog speed signals from the BL2600 SBC to the motor 
controllers. The Jumper J1 was included to allow the user to choose one half of the 12.0 
V power via the voltage divider to set the thresholds of the Schmitt trigger. 
Alternatively, one could use an external precision voltage reference connected to VCC if 
the user desired greater accuracy. 


115 





Figure 45. PWM Schematic Page 2. 
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APPENDIX B - ELECTRICAL WIRING COLOR CODES AND 

LABELS 


A wiring color code was implemented using the code deseribed in the table 
below. Orange Alfa wire, in 16 AWG, was used for main power supply wiring. It was 
not available in multiple colors, so extensive efforts were made to label wires for ease of 
maintenance. The robot’s electrical wires were numbered to assist researchers in 
maintaining and troubleshooting eleetrieal conneetions. The numbering seheme was 
implemented early in the development of the Creature, before many of the devices were 
coneeived, let alone installed. Beeause of this, the numbering is not sequential. The 
numbering seheme left many numbers unused allowing them to be assigned to deviees as 
they were installed. Some effort was made to logieally group numbers by function. 
Five-volt electronics bus supplies were assigned alphabetical labels as a mnemonic to aid 
in identifying their function. 


Wire Function 

Color 

Ground 

Blaek 

5 V supply 

Red 

12 V supply 

Yellow 

Unregulated Nominal 16V supply 

Yellow 

Digital Communication (example I2C) 

Purple 

Digital Signal 

Blue 

Analog Signal (example motor speed) 

White 


Tables. Wire Color Codes. 


119 



Label 

Function 

1 

IR #1 analog range voltage 

2 

IR #2 analog range voltage 

3 

IR #3 analog range voltage 

4 

IR #4 analog range voltage 

5 

IR #5 analog range voltage 

6 

IR #6 analog range voltage 

23 

Motor 1 +24 V 

24 

Motor 1 OV 

30 

Motor 2 OV 

31 

Motor 2 +24V 

40 

I2C SDA 

41 

I2C SCL 

50 

Motor 3 OV 

51 

Motor 3 +24V 

58 

Motor Bus Ground 

59 

Motor Bus 24V Unregulated 

60 

Motor Bus Ground 

61 

Motor Bus 5V Regulated 

62 

Unregulated Input to 7812 

63 

7812 12V Regulator Output 

64 

Electronics Bus Ground 

65 

Electronics Bus 12V Regulated 

66 

Unregulated 16V Electronics 

B 

RabbitLink Supply 16 V Unreg. 

C 

I2C Compass 5.0 V Regulated 

D 

BL2600 +K external pullup voltage 

I 

IMU 5.0 V Regulated 

R 

Router 5.1V Regulated 

S 

Sonar 5.1V Regulated 

V 

Sonar Servo 5.1V Regulated 

w 

I2C Wheel Tach 5.0 V Regulated 

X 

IR Sensors 5.0 V Regulated 


Table 6. Electrical Wiring Function to Label Cross-reference. 
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APPENDIX C - CREATURE OPERATING MANUAL 


The complex and unique nature of the Creature’s electrical power supply 
prompted the researcher to write a short user’s manual for the robot. The purpose of the 
manual is to allow future researchers to continue further research into operating program 
code enhancements, navigation, and obstacle avoidance without having to spend time 
tracing wires and deciphering the functions of various switches and fuses. Familiarity 
with the java GUI, pioneered by Uzun and running on the operator’s laptop, is assumed. 

Operating the Creature involves two start-up sequences: the vehicle and the laptop 
GUI. Before starting up, though, please ensure the Creature’s batteries have been 
charged. See the Motor Battery Charging and Electronics Battery Charging Procedures 
for details. Following the start up sequence given in Figures 46 through 49 will help 
ensure sensitive electronic devices are not subjected to voltage transients. 
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MOTOR BATTERY CHARGING 

Main Power Panel Motor Switch.OFF 

Motor Battery Qiaigc Panel Charge Switch.OFF/CHARGE 

Motor Battery Clratge Panel Series Switch.OFF 

Battery A Positive to Charger Positive Lead.CONNECTED 

Battery A Negative to Chaiger Neg. Lead..CONNECTED 

Rejieal for Bailery B 


Note 

Creature uses 10 C-cell NiMH batteries. Capacity 4000 mAH. Set 
Charger appropriately 

Battery A Charger.ON 

Battery B Cliargcr.ON 


Note 

Charging time varies depending on initial charge stale Charging 
times of 3 to 4 hours are common at 0,8 A charging current. 

When charging complete... 


Battery A (7harger.OFF 

Battery B Charger...OFF 

Batteiy A Positive Lead...^DISCONNECT 

Battery A Neg. Lead.. ^DISCONNECT 

Repeal for Baltery B 
If robot is to be operated... 

Robot Pre-Start Sequence.CONIPLETE 


ELECTRONICS BATTERY CHARGING PROCEDURE 

Note 

Electronics Battery has an isolated supply and ground from the 
Motor Baltery. All batteries may be charged simultaneously 

Caution 

Failure to turn off MPP Motor Power Switch prior to shutting down 
CPU can result in uncommanded motor operation at full speed 
Damage to robot could result if not on blocks. 


Main Power Panel Molor Switch.OFF 

Main Power Panel CPU Switch...OFF 

5V Bus Switches.OFF 

Main Power Panel BUS Switch..C>FF 

MPP Electronics Battery Supply Coni.E)ISC(DNNECT 

Electronics Battery Supply Cord at Ball...DISCONNECT & 

RHMOVH 


Creature Operating Manual Page 1. 
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Figure 46. 





















PowerStationl 00 Charger 


PT.UGTNTO 

BATTERY 


PowerStationl 00 Charger 


.PLUG INTO AC 
OLITLET 


Note 

Electronics Battery charge times can be as long as 4 hours. Green 
100% LED indicates full charge. See PowerStationl00 manual for 
more information. 


Electronics Battery Visual Indicator...MONITOR FOR 

RED LKU 

When Cliarging completed .. .. 

PowerStationl 00 Charger..DISCONNECT 

.FROM AC 

PowerStationl 00 Charger..UNPLUG FROM 

BAIT. 

Electronics Battery Supply Cord at Batt...INSTALL INTO 

.BATTERY 


Note 

Leave Electronics Battery Supply Cord disconnected from MPP until 
ready to operate. Connecting to the MPP can allow linear voltage 
regulators to begin drawing current and drain battery when robot is 
notin use 


Robot Pre-Start 

Motor Battery Cliarging..AS REQUIRED 

Electionics Battery Charging..AS REQUIRED 


Figure 47. 
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ROBOT START 

Main Power Panel Motor Switch 


OFF 


Caution 

Applying 24V to motors without CPU operating can result in 
uncommanded motor operation at full speed. Damage to robot 
could result if not on blocks. 

Motor Battery Charge Panel Charge Switch.ON/RLIN 

Motor Battery (Cliaigc Panel Scries Switch.SERIES 


Aux. Power Cord at MPP..DISCONNECT 

AS RHQ. 

Wires, leads, programming cables, etc..(’llIvCX 

.REMO\^D 

Note 

If robot is to be operated motionless or on blocks, test leads and 
programming cables can be left installed. 

Main Power Panel BUS Switch..OFF 

Main Power Panel CPU Switch..OFF 

Kleciroiiics Battery Supply (’ord at Batl..Pl.lJCKiHII) INTO 

.DATT. 

Electronics Battery Supply Cord at MPP..INSTALL 

Main Power Panel BUS Switch..ON 

5V Bus Sonar Switch..ON 

5V Bus Servo Switch ON 


Note 

Failure to apply power to the Sonar Sensor Head before activating 
the Router will cause the Router to hang or fault. If this happens, 
cycle router power If CPU is operating, it may be necessary to 
cycle power to CPU also. 

5V Bus Router Switch..ON 

Laptop WiFi and TCP/IP Setup..COlVfPLETE 

Note 

If the Creature wireless A.P. shuts down due to insufftclent SV 
supply, the Creature WiFi LAN will become unavailable on the 
laptop. Investigate 5V Bus supply. 

5V Bus RabbitLink Switch.ON 

Main Power Panel CPU Switch..ON 

5V Bus I2C CoiTipass/'I’acli Switch....../IN 

5V Bus IMtJ/IR Switch.’.ON 

Main Power Panel Motor Switch ON 


Figure 48. Creature Operating Manual Page 3. 
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ROBOT SHUTDOWN 

Main Power Panel Motor Switch.OFF 

5v Dus iMu/iR Switch. 

5V Bus Sonar Switch...OFF 

5V Bus Servo Switch..OFF 

5V Bus I2(.’ (.\)nij)ass/'l ach Switch.OFF 

5V Bils Rabbit!,ink Swutch..OF'!'' 

5V Dus Router Switch.OFF 

Main Pow'cr Panel BUS Switch...OFF 

Main Power Panel CPU Switch..OFF 


Motor Battery Cliarge Panel Charge Switch.OFF/CHARGE 

Motor Battery (Cliarge Panel Scries Switch.OFF 


LAPTOP WIFI AND TCP/IP SETUP 

l.<i|>lo|) luni oil..AS RHQ. 

Mac Airport or Windows Wireless Networking..ON 

When Creature WiFi A.P available.. .. 

Creature Access Point (A.P)...CONNECT TO 

DHCP.OFF 

IPv4.MANUAL 

TCP/IP Router IP Address.192.168.4.47 SET 

Laptop IP Address.192.168.4.37 SET 


Figure 49. Creature Operating Manual Page 4. 
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APPENDIX D - WHEEL TACHOMETER ASSEMBLY LANGUAGE 

CODE 


The PIC assembly language code below, specifically the ISR and I2C 
communications code and subroutines, has been used with the permission of the code’s 
original author to implement the tachometer functionality of the device installed in the 
Creature [38]. 


; pic3Wheel2ONOV.asm 
; By Kirk Volland 
; 11/20/2007 

; based on I2C ISR code written by 
; by Michael Gasperi 
; used with the author's permission 
; 6/11/2007 fixed per 16f690 errata on I2C port 

; times for 250 ms and checks for changes (transitions hi to lo or lo to hi) 
; on three digital input pins. Counts transistions, multiplies by 2. 

; When receives i2c read it writes the count to i2c bus. 

tinclude <pl6f690.inc> ; Change to device that you are using, 

tdefine NODE_ADDR OxDO ; I2C address of this node 64 decimal 


tdefine WHLIOLD 
tdefine WHL20LD 
tdefine WHL30LD 


wheelTachLast,5 
wheelTachLast,4 
wheelTachLastB, 7 


tdefine WHLl 
tdefine WHL2 
tdefine WHL3 


wheelTachNow,5 
wheelTachNow,4 
wheelTachNowB, 7 


; Variable declarations 


cblock 0x20 
WREGsave 
STATUSsave 
FSRsave 
PCLATHsave 

Temp ; used to decode ISR 

RegSel ; RegSelect 

DataRegs:7 

invalidWheelDat 

startValues 

wheelTachNow 

wheelTachNowB 

WheelTachLastB 

wheelTachLast 

wheel_l_cnt 

wheel_2_cnt 

wheel_3_cnt 

i 


endc 
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; 0x0001 


org 0 

goto Startup 
org 004 

goto ISR ; 0x0004 


; Main Code 


Startup 


; set up 

oscillator freq 


bsf 

STATUS,RPO 

; Register Bank 1 

bcf 

STATUS,RPl 

; Register Bank 1 

movlw 

b'01110001' 

; 8 MHz internal oscillator 

movwf 

OSCCON 


bcf 

STATUS,RPO 


bcf 

STATUS,RPl 


clrf 

PORTC 


banksel 

TRISC 


movlw 

OxFF 

; C all input 

movwf 

TRISC 

; config port C 

movlw 

OxFF 

; all input 

movwf 

TRISA 

; config port A 

movlw 

0x00 

; A2 D off 

banksel 

ADCONl 


movwf 

ADCONl 


movlw 

0x00 

; all port a digital 

banksel 

ANSEL 


clrf 

ANSEL 


movlw 

0x00 

; select channel 1 on A2D 

banksel 

ADCONO 

; A2 D off 

movwf 

ADCONO 


banksel 

PCON 

; power control 

bsf 

PCON,NOT_POR 


bsf 

PCON,NOT_BOR 


banksel 

PORTB 


clrf 

PORTB 

; Clear port B 

banksel 

PIRl 


clrf 

PIRl 

; Clear Tnterupts 

banksel 

TRISB 


movlw 

Oxff 


movwf 

TRISB 

; Port B all input 

banksel 

ANSEL 


clrf 

ANSELH 

; Port B all digital 
; set up TIMERl 

bcf 

STATUS,RPO 


bcf 

STATUS,RPl 

; Bank 0 

clrf 

TICON 

; reset it 

clrf 

TMRIH 

; clear the 2 bytes of counters 

clrf 

TMRIL 


movlw 

b'00110000' 

; prescaler 1:8 for long timing 250 ms 

movwf 

TICON 

; Use internal clock source. Keep TTMERl 


bcf 

TICON,0 

; TIMERl disable 

clrf 

PIRl 

; clear TMRl overflow flag 

clrf 

TMRIH 

; and Timerl hi and lo count bytes 

clrf 

TMRIL 


movlw 

OxOB 
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movwf 

TMRIH 

; load TIMRl with value = 3036 

movlw 

OxDC 

; so count up from 3036 to 65535 

movwf 

TMRIL 


bsf 

TICON,0 

; enable TIMERl and start counting up 

banksel 

SSPCON 


movlw 

0x39 

; setup ssp module workaround sequence 

movwf 

SSPCON 

r 

movlw 

0x36 

; Setup SSP module for 7-bit 

movwf 

SSPCON 

; address, slave mode 

movlw 

NODE_ADDR 

; set Node Address 

banksel 

SSPADD 


movwf 

SSPADD 


banksel 

SSPSTAT 


clrf 

SSPSTAT 

; clear SSP status 

banksel 

PIEl 

; Enable interrupts 

bsf 

PIEl,SSPIE 


bsf 

PIEl,TMRIIE 

; Enable timerl interrupt 

bsf 

INTCON,PETE 

; Enable all peripheral interrupts 

bsf 

INTCON,GTE 

; Enable global interrupts 


Do250msLoop 

bcf STATUS,RPO 
bcf STATUS,RPl 

clrf wheelTachNow ; clear the variable for holding PORTC results 
clrf wheelTachNowB 

movf PORTC,w ; put the current tachsignal from POTRC into 

; WREG 

movwf wheelTachNow ; make a copy of current values in case it 

; changes in next few cycles 


movf 

PORTB f w 




movwf 

wheelTachNowB 



Testl 

btf sc 
goto 

WHLl 

WhlTachlSet 

r 

do this if current wheel 1 tach signal is LO 


btf ss 

WHLIOLD 

r 

was the last wheel 1 tach signal LO? 


goto 

Test2 

r 

if both are LO, then no change, go to wheel 2 

test 

incf 

wheel_l_cnt, 

f ; 

increment if new == 0 and old == 1 


goto 

Test2 




WhlTachlSet 

btf sc 

WHLIOLD 

r 

current tach is HI. Was last tach HI, too? 


goto 

Test2 

r 

if current == 1 and old == 1, then no change. 

go 

incf 

wheel_l_cnt, 

r 

f 

to Tests 

; new not equal to old, so increment the 

count 

Test2 

btf sc 
goto 

WHL2 

WhlTachlSet 

r 

do this if current wheel 1 tach signal is LO 


btf ss 

WHL20LD 

r 

was the last wheel 1 tach signal LO? 


goto 

Tests 

r 

if both are LO, then no change, go to wheel 3 

test 

incf 

wheel_2_cnt, 

f 

; increment if new == 0 and old == 1 


goto 

Tests 




WhlTach2Set 

btf sc 

WHL20LD 

r 

current tach is HI. Was last tach HI, too? 


goto 

Tests 

r 

if current == 1 and old == 1, then no change. 

go to 

incf 

wheel_2_cnt, 

r 

f 

end. copy values to old 

; new not equal to old, so increment the 

count 

Tests 

btf sc 
goto 

WHL3 

WhlTachSSet 

r 

do this if current wheel 1 tach signal is LO 


btf ss 

WHL30LD 

r 

was the last wheel 1 tach signal LO? 
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goto CopyVals ; if both are LO, then no change, go to copy values 

incf wheel_3_cnt,f ; increment if new == 0 and old == 1 

goto CopyVals 
WhlTach3Set 

btfsc WHL30LD ; current tach is HI. Was last tach HI, too? 

goto CopyVals ; if current == 1 and old == 1, then no change, go 

; to end of loop, copy values 

incf wheel_3_cnt,f ; new not equal to old, so increment the count 

CopyVals 

movf wheelTachNow, w ; end of loop 

movwf wheelTachLast ; save current values as old values before 

; restart loop 

movf wheelTachNowB,w 
movwf wheelTachLastB 

movlw OxOB ; count down from 27 to zero 

movwf i ; delay the check loop for about 40 usee before checking 

deefsz i, f ; again to allow for signal rise times, 

goto $-1 

clrwdt 

goto Do250msLoop ; Do it again 


; Generic Interrupt Service Routine 


movwf 

WREGsave ; 

Save WREG 

movf 

STATUS,W ; 

Get STATUS register 

banksel 

STATUSsave 

Switch banks, if needed. 

movwf 

STATUSsave 

Save the STATUS register 

movf 

PCLATH,W ; 


movwf 

PCLATHsave ; 

Save PCLATH 

movf 

FSR,W ; 


movwf 

FSRsave ; 

Save FSR 

banksel 

PTRl 


btfsc 

PIR1,SSPIF ; 

Is this a SSP interrupt? 

goto 

HandleSSP 


btf ss 

PIR1,TMR1TF ; 

Is it a Timer 1 overflow? 

goto 

$ 

No, just trap here. 

bef 

PIR1,TMR1IF ; 

clear interrupt for 250ms loop in timerl 

movlw 

OxOB 


movwf 

TMRIH ; 

reload TIMRl with value = 3036 

movlw 

OxDC 

so count up from 3036 to 65535 

movwf 

TMRIL 


bef 

STATUS,C 

r 

since timing for 1/4 second and counting 
every half cycle 

rlf 

wheel_l_cnt,w ; 

r 

multiply wheel count by 4 and divide by to 
2 to get number cycles /I sec 

movwf 

DataRegs+l 


clrf 

wheel_l_cnt ; 

zero the current count for wheel 1 

bef 

STATUS,C 

r 

since timing for 1/4 second and counting 
every half cycle 

rlf 

wheel_2_cnt,w ; 

r 

multiply wheel count by 4 and divide by 2 
to get number cycles /I sec 

movwf 

DataRegs+2 


clrf 

wheel_2_cnt ; 

zero the current count for wheel 2 

bef 

STATUS,C 

since timing for 1/4 second and counting 


; half cycle every 
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rlf 

wheel_S_cnt , w; 

multiply wheel count by 4 and divide by 


r 

to get number cycles /I sec 

movwf 

DataRegs+S 


clrf 

wheel_S_cnt 

; zero the current count for wheel 

movf 

PORTC,w 


movwf 

wheelTachLast 

; seed the old value 

movf 

PORTB f w 


movwf 

wheelTachLastB 

; seed the old value 

goto 

RestoreFromTSR 


HandleSSP 

bcf 

PIRl,SSPIF 


call 

SSP_Handler 

; Yes, service SSP interrupt. 

RestoreFromTSR 

banksel 

FSRsave 


movf 

FSRsave,W 


movwf 

FSR 

; Restore ESR 

movf 

PCLATHsave,W 


movwf 

PCLATH 

; Restore PCLATH 

movf 

STATUSsave,W 


movwf 

STATUS 

; Restore STATUS 

swapf 

WREGsave,F 


swapf 

WREGsave,W 

; Restore WREG 

retfie 


; Return from interrupt . 

SSP_Handler 

banksel 

SSPSTAT 


movf 

SSPSTAT,W ; 

Get the value of SSPSTAT 

andlw 

b'OOlOllOl' 

Mask out unimportant bits in SSPSTAT. 

banksel 

Temp ; 

Put masked value in Temp 

movwf Temp 

r 

for comparision checking. 

Statel: 

r 

Write operation, last byte was an 

movlw 

b'OOOOlOOl' 

address, buffer is full. 

xorwf 

T emp , W ; 


btf ss 

STATUS,Z ; 

Are we in Statel? 

goto 

State2 ; 

No, check for next state . 

banksel 

SSPBUE 


movf 

SSPBUE,W ; 

Get the node addr and throw it away 

movlw 

0x00 ; 

initalize register select to 0 


r 

so first write will go into itself 

movwf RegSe 

1 



return 

State2 : 

movlw 

xorwf 

btf ss 

goto 

banksel 

movf 

banksel 

movwf 

return 

states : 

movlw 
xorwf 
btf ss 
goto 
bcf 


b'00101001' 
T emp,W 
STATUS,Z 
states 
SSPBUF 
SSPBUF,W 
DataRegs 
DataRegs 


Write operation, last byte was data, 
buffer is full. 

Are we in State2? 

No, check for next state. 


Get the byte into W 

save the byte to 0th array location 


b'00001100' 
T emp,W 
STATUS,Z 
State4 
STATUS,Z 


First Read operation, last byte was an 
address, buffer is empty. 

Are we in StateS? 

No, check for next state. 


131 







; case switch is the value of DataRegs == 3 



movf 

DataRegs, w 



xorlw 

3 



btf ss 

STATUS,Z 



goto 

NotEq3 

; DataRegs = 3 so send out 4th array element 


movf 

DataRegs+3, 

w ; save to WREG for output 


goto 

StateSSend 


NotEqS 

xorlw 

2^3 

; if DataRegs != 3 is it 2? 


btf ss 

STATUS, Z 



goto 

NotEq2 

; DataRegs = 2 so send out 3th array element 


movf 

DataRegs+2, 

w ; save to WREG for output 


goto 

StateSSend 


NotEq2 



; default if not == 2 or 3, then DataRegs == 


movf 

DataRegs+1, 

w ; save array element 2 to WREG 

StateSSend 




call 

return 

WriteI2C 

; Write the byte to SSPBUE 

State4 

movlw 

b'00101100 ' 

; Other reads operation, last byte was data, 

; buffer is empty. 


xorwf 

T emp,W 



btf ss 

STATUS,Z 

; Are we in State4? 


goto 

states 

; No, check for next state.... 


banksel 

RegSel 



movlw 

OxA 



call 

return 

WriteI2C 

; Write the byte to SSPBUE 

states 

movlw 

b'00101000' 

; A NACK was received when transmitting 


xorwf 

T emp, W 

; data back from the master. Slave logic 


btf ss 

STATUS,Z 

; is reset in this case. R_W = 0, D_A = 1 


goto 

I2CErr 

; and BE = 0 


return 

; If 

we aren't in StateS, then something is wrong. 

I2CErr 

nop 

goto $ 


; let watch dog catch this 


return 


; WriteI2C 


WriteI2C 

banksel 
btf sc 
goto 
banksel 
DoI2CWrite 
bcf 
movwf 
btf sc 
goto 
bsf 

return 

end 


SSPSTAT 
SSPSTAT,BF 
WriteI2C 
SSPCON 


; Is the buffer full? 
; Yes, keep waiting. 

; No, continue. 


SSPCON,WOOL 
SSPBUF 
SSPCON,WOOL 
DoI2CWrite 
SSPCON,CKP 


; Clear the WCOL flag. 

; Write the byte in WREG 
; Was there a write collision? 

; Release the clock. 


; End of file 
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APPENDIX E - SONAR SENSOR HEAD CONTROLLER 
ASSEMBLY LANGUAGE CODE 


r 

; sonarServoHeadl1.asm 

r 

; version 11 from sonarServoHeadlO.asm 
; Change History: 

; version 11 adjusts time delay between sonar numbers 1 and 4 
; to start motors moving then do delay for false echoes. 

; ver7b added longer delay when moving to position 5 to give servo 
; Time to move the longer distance. Delays 160 ms. 

; Changed delay when moving between sectors to 80 ms from 60 ms. 

; ver7a 

; Coverted back to one byte data format per range reading. 

; Added one byte at end of range sentence to indicate which element in 
; array is the newest (i.e. time late = 0). Other time lates can be 
; figured from that one. 

; Added test of 16 bit count. If MSB is set, then range > max. reportable. 

; Also, if downconverted byte > 250, then sets value to 0x02 for no contact. 
; Fine tuned the sonar firings, waits 40 ms between each sonar firing 
; at a given position. 

; Changed to 4 sonars and 5 servo positions. 

; Moves sonar head to 5 positions. 

; Generates 20 sonar range values 


; Kirk Volland 
; 19 OCT 2007 

r 

; Hardware: PIC16F690 running at 8MHz 
; Sonar 1 INIT output on RA2 

; Sonar 2 INIT output on RB6 

; Sonar 3 INIT output on RA5 

; Sonar 4 INIT output on RB4 

; Blanking Inhibit BINH output on RA4 
; Combined OR'ed ECHO line input on RC5 
; Servo control output on RCO 

r 

r 

; TIMERO prescaler set to 1:16 so one 'tick' is 8 usee 
; use TIMERO for the servo pulse widths 
; TIMERl prescaler set 1:1 so one 'tick' is .5 usee 

t 

tdefine ECHOPIN PORTC,5 
tdefine PULSEONDELAY 0xB2 


tinclude <pl6F690.inc> 

_config (_INTRC_OSC_NOCLKOUT & _WDT_OFF & _PWRTE_OFF & 

_MCLRE_OFF & _CP_OFF & _BOR_OFF & _IESO_OFF & _FCMEN_OFF) 


; variables 

cblock 0x20 


j 

pulseCount 
SonarRange:2 
SonarRanges:20 
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PulseCount 

Position 

SonarNum 

tempVal 

Arrindex 

endc 
; macros 

servoHiMac macro 

movlw 0x01 

movwf PORTC 

endm 


servoLoMac 

nop 

nop 

nop 

nop 

movlw 

movf 

nop 

nop 

nop 

endm 


macro 


0x0 

PORTC,f 


PingSonar4Mac macro 

movlw b'OOOlOOOO' 

movwf PORTB 

endm 

PingSonarSMac macro 

movlw b'OOlOOOOO' 

movwf PORTA 

endm 

PingSonar2Mac macro 

movlw b'OlOOOOOO' 

movwf PORTB 

endm 

PingSonar IMac macro 

movlw b'OOOOOlOO' 

movwf PORTA 

endm 

org 0 
nop 


.**************************** Initialization 
Init 

; set up oscillator freq 
bsf STATUS,RPO 

bcf STATUS,RPl 

movlw b'OlllOOOl' 
movwf OSCCON 




; Register Bank 1 
; Register Bank 1 
; 8 MHz internal oscillator 


bcf 

STATUS,RPO 



bcf 

STATUS,RPl 


; Bank 0 

clrf 

CCPICON 


; clear capture 
; turn it off 

clrf 

PORTA 

; init 

portA 

clrf 

PORTB 

; init 

portB 

clrf 

PORTC 

; init 

PORTC 


; make all input and outputs digital 

bsf STATUS,RPO ; Register Page 

bsf STATUS,RPl ; Register Page 


compare module. 


2 

2 


134 



clrf 

ANSEL 

r 

all digital 10 

clrf 

ANSELH 



; set 

up TIMERl 



bcf 

STATUS,RPO 



bcf 

STATUS,RPl 

; Bank 0 


clrf 

TICON 

; reset 

it 

clrf 

TMRIH 

; clear 

the 2 bytes of counters 

clrf 

TMRIL 



movlw 

0x0 

; prescaler 1:1 for Sonar Range timer 

movwf 

TICON 

; Use internal clock source. Keep TIMERl off 

; set 

up TIMERO 



bsf 

STATUS 

,RP0 

Bank 1 

movlw 

b'OOOOOOll' 

Sourced from the Processor clock 

movwf 

OPTION 

_REG 

Bits 2,1,0 are 011, so prescaler 1:16 or 



r 

8 usee per bit for Servo Pulse HI time 

; set 

up pins for Inputs and 

Outputs 

movlw 

b'OOlOOOOO' 

All output except capture comparator ccpl 

movwf 

TRISC 

r 

RC5 input (combined ECHO all others out 

clrf 

TRISB 

r 

port B all output 

clrf 

TRISA 

r 

port A all output 



; intialize baud rate set up 57.14 kbit 



; Eosc = 

8MHz 



; SYNC = 

0, BRGH= 1, BRG16 = 1, SPBRG = .34 

movlw 

.34 



movwf 

SPBRG 



bsf 

BAUDCTL, BRGI6 

; set BRG16 of BAUDCTL register 

bsf 

TXSTA -'0x80, 

BRGH ; 

BRGH set HI 

bcf 

TXSTA -'0x80, 

SYNC 

SYNC LO , enable serial port asych. 

bcf 

TXSTA -'0x80, 

TX9 

; 8bit data 

bsf 

TXSTA "^0x80, 

TXEN 

; enable Transmiter 

bcf 

STATUS,RPO 


back to Register Page 0 

bsf 

RCSTA, SPEN 

; enable 

serial port 

bcf 

RCSTA, CREN 

; continuous recv. disable 

bsf 

RCSTA, RX9 

r 

9 bit parity 

movf 

RCREG, w 


read RECREG to clear any pending IRQ 

.-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k Begin Main Loop ************************ 

StartPosition 



movlw 

0x05 

r 

load the starting position variable = 5 

movwf 

Position 

r 

position 5 is 6 degrees from far R. limit 

MoveServos 


r 

test if servo is moving to position 5 

nop 


r 

if yes, allow more time for servo to move 

bcf 

STATUS,Z 

r 

the bigger distance before firing off sonar 

movf 

Position,w 

r 

Position to WREG 

xorlw 

0x05 

r 

Is Position == 5 ? 

btf ss 

STATUS,Z 



goto 

SetDelay3 

r 

if NOT, then skip to below 

movlw 

0x09 

r 

want 9 * 20 ms delays = 180 ms 

goto 

SaveDelay 

r 

for servo to move 72 degrees 

SetDelayS 




movlw 

0x03 

r 

Only moving 18 degrees. Three 20 ms delays 

SaveDelay 


r 

before any ranging attempt. 3* 20ms = 60 ms 

movwf 

PulseCount 

r 

of 'dead' cycles with no sonar pinging 


SendsignalsAndWait 

call SubSendServoPulse ; send out HI pulse on the servo control pin 

; duration of HI pulse depends on Position 
call SubDEL20 ; do a 20 ms delay 

decfsz PulseCount,f ; Count down from 3 to zero, then begin 
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goto 


SendSignalsAndWait ; ranging with sonars, else do another 

; 'dead' cycle with no sonar firing. 


movlw 0x4 
movwf SonarNum 


; Do ranging with each of 4 sonars, one at a time 
; load counter with initial value = 4 
; to keep track of which sonar is being 
; pinged. 


RangingAttempt 
nop 

call SubSendServoPulse ; must send out a pulse to the servo first 

nop ; then we have 20 to 30 ms to use for sonar 

; ranging attempt before the servo needs 
; another signal 


; case switch logic to pick which sonar to send INIT pin HI out 
movf SonarNum,w 
xorlw 4 
btfss STATUS,Z 
goto NotSonar4 
PingSonar4Mac 
goto PingSent 
NotSonar4 

xorlw 4^3 
btfss STATUS,Z 
goto NotSonar3 
PingSonar3Mac 
goto PingSent 
NotSonar3 

xorlw 3^2 
btfss STATUS,Z 
goto NotSonar2 
PingSonar2Mac 
goto PingSent 
NotSonar2 

PingSonarlMac 


PingSent 

; set up Capture Compare to catch rising edge of echo 


clrf 

CCPICON 

movlw 

b'00000101' 

movwf 

CCPICON 

bef 

TICON,0 

clrf 

PIRl 

clrf 

TMRIH 

clrf 

CCPRIH 

clrf 

TMRIL 

clrf 

CCPRIL 

■k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k 

bsf 

TICON 


Ping, 


; capture the RISING edge of echo signal 
; when it's passed through XOR-gate 

; TIMERl disable 
; clear TMRl overflow flag 
; and Timerl capture flag 
; clear high and low bytes of counters 
; clear the capture compare values, too 
; before next ranging 
sent start timing ********************** 
; enable TIMERl 


OneMsDelay 


movlw 


0x04 

movwf 


j 

movlw 


0xA7 

movwf 


i 

deefsz 


i, f 

goto 

$-1 


deefsz 

if f 


goto 

$-5 



; delay 1ms for transducer to stop ringing 
; do 4 delays 

; 167* 3 instructions/loop * 0.5 usee = 250 usee 
; delay to allow for sonar transducer ring down 
; and allow time for the "crud" and " 

; "glitches on the echo 
; line to go away 
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; send blanking inhibit to override the default 2.38 ms blanking 
movf SonarNum,w 

xorlw 3 ; is it blanking inhibit for sonar 3? 

btfss STATUS,Z 

goto NotBINHforSonar3 

movlw b'00110000' ; send this value out PORTA if it's sonar3 

movwf PORTA 
goto BINHsent 
NotBINHforSonar3 
xorlw 3^1 

btfss STATUS,Z ; it's not sonar 3. Is it sonarl? 

goto NotBINHforSonarl 

movlw b'00010100' ; send BINH pin HI and keep the sonar INIT high 

movwf PORTA ; for sonar 3 

goto BINHsent 
NotBINHforSonarl 

movlw b'OOOlOOOO' ; Not sonars 1 or 3. Must be 2 or 4. 

movwf PORTA ; regardless, we're not pinging a sonar in PORTA 

; so just sent the BINH pin in PORTA hi. 

BINHsent 

nop ; done 

clrf PIRl ; clear TMRl overflow flag. 

;MAKE SURE CAPTURE ELAG IS CLEAR 
; AETER THE BINH SIGNAL GETS SENT. 

check_echo 

btfsc PIRl,2 ; check for capture of rising edge of echo 

goto check_done ; if capture flag is HI, then get out of loop 

btfss PIR1,0 ; check for over flow of TIMERl count 

goto check_echo ; if overview flag not set, then go back up and do 

; again 

goto ovr_flo ; TIMERl must have overflowed 


check_done 

bef TICON, TMRION ; stop TIMERl counter 

movf CCPRlL,w ; copy the values of capture compare 

movwf SonarRange ; counters to vars. Keep 7 bits of high byte 

; and MSB of LO byte. 

movf CCPRlH,w ; copy high byte of the couter to high byte of 

; range variable 

movwf SonarRange+1 

btfsc SonarRange+1,7 ; test the MSB of HI byte. See if value will 

; overflow 
; 1 byte output. 

goto ovr_flo ; if set, then value is too big to represent it 

; with 1 byte. 

rlf SonarRange,w ; shift 7th bit out of LSByte and into carry 

; Elag C 

rlf SonarRange+1,f ; shift the carry flag value into bit 0 of 

; MSByte and 

; shift MSBit out of register to 
; the carry Elag C 

; i.e. throw out the 2''16th counter value. 

; Keep 2''8 thru 2^15 bits. 

bsf TICON, TMRION ; restart TIMERl counter 

bef PIRl,2 ; reset the capture compare flag 

btfss PIR1,0 ; continue timing to the end of normal 

goto $-1 ; listening period so that next servo 

; pulse will be about 20 to 30 ms after 
; RangingAttempt 
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goto done 


ovr flo 


movlw 0x02 
movwf SonarRange 
movwf SonarRange +1 


; set equal to 2 for no contact 
; set equal to 0x02 for no contact flag 


.********** *********** Done with Timing *********************** 
done 


movf SonarRange+1,w 
sublw OxFA 

btfsc STATUS,C 
goto saveValueToArr 
movlw 0x02 
movwf SonarRange+1 


do sanity check on the 1 byte range value 

subtract WREG(SonarRange) 

from literal 250, result to WREG 

test carry flag. Is SonarRange+1 byte > 250? 

skip ahead and save value to array 

else... 

if value 251 to 255, report as no 
contact 


SaveValueToArr 


movf Position,w 

sublw 5 

movwf Arrindex 

bcf STATUS,C 

rlf Arrindex, f 

bcf STATUS,C 

rlf Arrindex,f 


store the range value in a unique spot in the array 

Put sonar reading in an array location depending on which 

sonar fired and which position it was pointed at. 

the 2^3 and 2^2 bits indicate the position 

the 2^1 and 2^0 bits indicate the sonar number (front, 

rear, L, R) 

array index = 4*(5 - Position) + (4 - SonarNum) 
w ; load WREG with current Position 

; find difference 5 - Position put into WREG 
; make a copy put into Arrindex 
ATUS,C ; carry value = 0 

rindex,f ; shift value to left. i.e. multiply by 2 

; carry value = 0 

rindex,f ; shift 0 into bit 0 and others over to the 
; left. i.e. multipy by 2 again 


SonarNum,w 
4 


addwf Arrindex,f 


movf SonarNum,w ; load WREG with sonar number that was pinged 

sublw 4 ; find 4 - SonarNum and put result in WREG 

addwf Arrindex,f ; sum WREG + Arrindex = Arrindex 

; store high byte in SonarRanges array at Arrindex location 
movf Arrindex,w 
addlw SonarRanges 

movwf FSR ; Set FSR to the array index offset + SonarRanges 

; file register location 

movf SonarRange+1,w ; want to save the High byte of SonarRange 


movwf INDF 

clrf PORTA 
clrf PORTB 


; move WREG to the array position 


; send all INIT pins and the BINH pin low 


nop 

; Done with delay. Decrement through sonars 4 

; do a sonar ranging "ping" for next sonar. 

; Same azimuth position. 

decfsz Position,f ; After all 4 sonars fired, decrement position. 

goto MoveServos ; Go back up to start of loop and move servos to 

; new position. 


NextNum 

decfsz SonarNum,f 

to 1 

goto RangingAttempt 
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; if Position == 0, then we've fired off 
; at positions 5,4,3,2,1. Need to reset to position to 

; 5. 

movlw 0x05 ; Reset position to position 5, start position 

movwf Position 

goto StartPosition ; Repeat loop and have servo motor reset to 

; the start position = far Right. 


. ********************* Subroutines ******************************* 
; Subroutine to send the RC servo pulse to the Futaba 3003 servo 
SubsendservoPulse 


clrf 

CCPICON 

; turn off the ccpl module 
; set up TIMERO 

bsf 

STATUS,RPO 

; Bank 1 

movlw 

b'00000011' 

; Sourced from the Processor clock 

movwf 

OPTION_REG 

; Bits 2,1,0 are 011, so prescaler 1:16 or 

; 8 usee per bit for Servo Pulse HI time 

bcf 

STATUS,RPO 

; bank 0 again 

bcf 

STATUS,Z 

; clear this in case it's set, want to check against 
; it later 

bcf 

INTCON,TOIF 

; clear the overflow flag and then... 

clrf 

TMRO 

; zero out TMRO count 

; case switch logic to find out which position 

movf 

Position,w 

; use Position var. to determine duration of 
; HI pulse to servos. 

; Position in degrees from full Left = 

;Case switch logic 
; test == 5? 

xorlw 

5 


btf ss 

STATUS,Z 


goto 

NotPosit5 



servoHiMac ; pulse the Servo pin HI for 105 "ticks" of 8usec each 
movlw 
movwf 
btf ss 
goto 
bcf 

goto 

NotPosit5 

xorlw 
btf ss 
goto 
servo 
movlw 
movwf 
btf ss 
goto 
bcf 
nop 
nop 
nop 
nop 
goto 

NotPositi 

xorlw 
btf ss 
goto 
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0x97 

TMRO 

INTCON,TOIF 
$-1 

INTCON,TOIF 


count up from decimal 151 to 256 

to keep pulse HI for 840 usee for 54 degree posit 


clear the overflow flag and then 


PulseDone 


5^4 

STATUS,Z 

NotPosit4 

HiMac 

0x80 

TMRO 

INTCON,TOIF 
$-1 

INTCON,TOIF 


test == 4? 


pulse the Servo pin HI count 127 ticks 
count up from 256 - 128.5 = 128 decimal 
to keep pulse HI for 1020 usee 
for 72 degree position 

clear the overflow flag and then ... 


PulseDone 


; add a few nop cycles to get just about 1020 usee 


4^3 

STATUS,Z 
NotPosit3 


test == 3? 

skip below code if Position is NOT 3 

DO this code if Position == 3 center = 90 deg 






r 

from far R. limit 


servoHiMac 

r 

pulse the Servo pin HI for 150 ticks 


movlw 

0x6A 

r 

count up from 256 - 150 = 106 to 256 


movwf 

TMRO 

r 

to keep pulse HI for 1200 usee 


btf ss 

INTCON,TOIF 




goto 

$-1 




bcf 

INTCON,TOIF 

r 

clear the overflow flag and then ... 


goto 

PulseDone 




NotPositS 





xorlw 

3^2 




btf ss 

STATUS,Z 




goto 

NotPosit2 





; DO 

this code if Position == 2 == 18 degrees L 

of neutral 


; or 

108 

degrees from far R. limit 


servoHiMac 

r 

pulse the Servo pin HI for 172.5 ticks 


movlw 

0x54 

r 

count up from 256 - 172.5 = 83.5 = 84 


movwf 

TMRO 

r 

to keep pulse HI for 1380 usee 


btf ss 

INTCON,TOIF 




goto 

$-1 




bcf 

INTCON,TOIF 

r 

clear the overflow flag and then... 


nop 





nop 





nop 

; add a couple 

of nop cycles to add a bit more time to 

servohi 

goto 

PulseDone 




NotPosit2 

; Must be Position == 1 == 126 degrees from Far 

R. limit 

servoHiMac 

r 

pulse the Servo pin HI for 1560/8 = 195 

ticks 

movlw 

0x3D 

r 

count up from from 256 - 195 = 61 


movwf 

TMRO 

r 

to keep pulse HI for 1560 usee 


btf ss 

INTCON,TOIF 




goto 

$-1 




bcf 

INTCON,TOIF 

r 

clear the overflow flag and then... 


nop 





nop 





nop 





nop 





PulseDone 





servoLoMac 

r 

set the servo to low again 


nop 





return 






; delays for 

20 ms using TIMERO. 

Sends Data via RS232. 

; Note prescaler change to 1:256. 


SubDEL20 



bsf 

STATUS,RPO 

; Bank 1 

movlw 

b' 00000111' 

; configure TimerO. 

movwf 

OPTION_REG 

; Prescaler 1:256 so 1 tick = 128 usee 

bcf 

STATUS,RPO 

; Bank 0 

movlw 

0x64 

; timerO count from 100 to 256 

movwf 

TMRO 

; so count 156 ticks = 19.97 ms 

btf ss 

INTCON,TOIE 

; wait for overflow 

goto 

$-1 


bcf 

INTCON,TOIE 


bsf 

STATUS,RPO 

; Bank 1 

movlw 

b' 00000011' 

; Reconfigure TMRO back to 1:16 

movwf 

OPTION_REG 


bcf 

STATUS,RPO 

; Bank 0 

; send out the range data to serial 
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bcf PIR1,TXIF ; clear bit 

; load WREG with literal value of OxFE 
movlw OxFE ; load WREG with flag to signal start of 

; sonar sentence 

movwf TXREG ; send the sonar range byte out serial 

nop 

nop 

btfss PIR1,TXIF 
goto $-1 

bcf PIR1,TXIF ; clear bit 

; repeat start flag 

movlw OxFE ; load WREG with flag to signal start of 

; sonar sentence 

movwf TXREG ; send the sonar range byte out serial 

nop 

nop 

btfss PIR1,TXIF 
goto $-1 

bcf PIR1,TXIF ; clear bit 

; BEGIN data section of serial transmission 
; Very Kludge. Could implement a counter and loop 
; to access the array values. 

; Data output begins with value that is closest to yaxis of robot 
; and proceeds clockwise from 6degrees to 24, to 42, etc. to last 
; value (array element 6) which is 348 degrees 

; element 2 

movf SonarRanges+0x02,w ; load WREG with 1 byte 

movwf TXREG ; send the sonar range byte out serial 

nop 

nop 

btfss PIR1,TXIF 

goto $-1 

nop 

bcf PIR1,TXIF ; clear bit 

; element 19 

movf SonarRanges+0xl3,w ; load WREG with 1 byte 

movwf TXREG ;send the sonar range byte out serial 

nop 

nop 

btfss PIR1,TXIF 

goto $-1 

nop 

bcf PIR1,TXIF ; clear bit 

; element 15 

movf SonarRanges+OxOF,w ; load WREG with 1 byte 
movwf TXREG send the sonar range byte out serial 

nop 
nop 

btfss PIR1,TXIF 

goto $-1 

nop 

bcf PIR1,TXIF ; clear bit 


; element 11 

movf SonarRanges+OxOB,w ; load WREG with 1 byte 

movwf TXREG ; send the sonar range byte out serial 

nop 

141 





; clear bit 


nop 

btfss PIR1,TXIF 

goto $-1 

nop 

bcf PIR1,TXIF 


; element 7 

movf SonarRanges+0x07,w ; load WREG with 1 byte 

movwf TXREG ;send the sonar range byte out serial 

nop 

nop 

btfss PIR1,TXIF 

goto $-1 

nop 

bcf PIR1,TXIF ; clear bit 


movf SonarRanges+0x03,w 

movwf TXREG 

nop 

nop 

btfss PIR1,TXIF 

goto $-1 

nop 

bcf PIR1,TXIF 


; element 3 

; load WREG with 1 byte 
; send the sonar range byte out serial 


; clear bit 


movf SonarRanges+OxlO,w 

movwf TXREG 

nop 

nop 

btfss PIR1,TXIF 

goto $-1 

nop 

bcf PIR1,TXIF 

movf SonarRanges+OxOC,w 

movwf TXREG 

nop 

nop 

btfss PIR1,TXIF 

goto $-1 

nop 

bcf PIR1,TXIF 


; element 16 

; load WREG with 1 byte 

; send the sonar range byte out serial 


; clear bit 
; element 12 
; load WREG with 1 byte 
; send the sonar range byte out serial 


; clear bit 


movf SonarRanges + OxOS, w 

movwf TXREG 

nop 

nop 

btfss PIR1,TXIF 

goto $-1 

nop 

bcf PIR1,TXIF 


; element 8 

; load WREG with 1 byte 
; send the sonar range byte out serial 


; clear bit 


movf SonarRanges+0x04,w 

movwf TXREG 

nop 

nop 

btfss PIR1,TXIF 


; element 4 

; load WREG with 1 byte 
; send the sonar range byte out serial 
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; clear bit 


goto $-1 
nop 

bcf PIR1,TXIF 


; element 0 


movf 

SonarRanges, w 

; load WREG with 

1 byte 



movwf 

TXREG 

; send the sonar 

range byte 

out 

serial 

nop 

nop 

btf ss 

PIRl,TXIF 





goto 

nop 

$-1 





bcf 

PIRl,TXIF 

; clear bit 






; element 17 




movf 

SonarRanges+Oxl1,w 

; load WREG with 

1 byte 



movwf 

TXREG 

; send the sonar 

range byte 

out 

serial 

nop 

nop 

btf ss 

PIRl,TXIF 





goto 

nop 

$-1 





bcf 

PIRl,TXIF 

; clear bit 






; element 13 




movf 

SonarRanges+OxOD,w 

; load WREG with 

1 byte 



movwf 

TXREG 

; send the sonar 

range byte 

out 

serial 

nop 

nop 

btf ss 

PTRl,TXIF 





goto 

nop 

$-1 





bcf 

PIRl,TXIF 

; clear bit 






; element 9 




movf 

SonarRanges+0x09,w 

; load WREG with 

1 byte 



movwf 

TXREG ; send 

the sonar range 

byte out serial 


nop 

nop 

btf ss 

PIRl,TXIF 





goto 

nop 

$-1 





bcf 

PIRl,TXIF 

; clear bit 






; element 5 




movf 

SonarRanges+0x05,w 

; load WREG with 

1 byte 



movwf 

TXREG 

; send the sonar 

range byte 

out 

serial 

nop 

nop 

btf ss 

PIRl,TXIF 





goto 

nop 

$-1 





bcf 

PIRl,TXIF 

; clear bit 






; element 1 




movf 

SonarRanges + OxO1, w 

; load WREG with 

1 byte 



movwf 

TXREG 

; send the sonar 

range byte 

out 

serial 

nop 

nop 

btf ss 

PTRl,TXTF 





goto 

$-1 






143 



nop 

bcf 


PIRl,TXIF 


; clear bit 


; element 18 


movf 

SonarRanges + 0xl2, w 

; load WREG with 

1 byte 



movwf 

TXREG 

; send the sonar 

range byte 

out 

serial 

nop 

nop 

btf ss 

PIRl,TXIF 





goto 

nop 

$-1 





bcf 

PIRl,TXIF 

; clear bit 






; element 14 




movf 

SonarRanges+OxOE,w 

; load WREG with 

1 byte 



movwf 

TXREG 

; send the sonar 

range byte 

out 

serial 

nop 

nop 

btf ss 

PTRI,TXIF 





goto 

nop 

$-1 





bcf 

PIRl,TXIF 

; clear bit 






; element 10 




movf 

SonarRanges+OxOA,w 

; load WREG with 

1 byte 



movwf 

TXREG 

; send the sonar 

range byte 

out 

serial 

nop 

nop 

btf ss 

PIRl,TXIF 





goto 

nop 

$-1 





bcf 

PIRl,TXIF 

; clear bit 






; element 6 




movf 

SonarRanges+0x06,w 

; load WREG with 

1 byte 



movwf 

TXREG 

; send the sonar 

range byte 

out 

serial 

nop 

nop 

btf ss 

PIRl,TXIF 





goto 

nop 

$-1 





bcf 

PIRl,TXIF 

; clear bit 






; indicate which 

byte is newest 




; i.e. time late 

= 0. 



movf 

Arrindex,w 

; load WREG with 

1 byte 



movwf 

TXREG 

; send the last byte out serial 


nop 

nop 

btf ss 

PTRI,TXIF 





goto 

nop 

$-1 





bcf 

PIRl,TXIF 

; clear bit 






; send end flag 




movlw 

OxFB 

; load WREG with 

flag to signal 

end of 



; sonar sentence 




movwf 

TXREG 

; send the sonar 

range byte 

out 

serial 

nop 

nop 

btf ss 

PTRI,TXIF 





goto 

$-1 





bcf 

PIRl,TXIF 

; clear bit 
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return 


end 
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APPENDIX F - DYNAMIC C ROBOT OPERATING CODE 


//creature6mod2.c 
//15NOV2007 


// Dynamic C 9.21 running on BL2600 
// ver5 adds IMU support and heading hold from IMU 
// ver6 turns on random sonar walk if contact within 40 cm 
// added IR speed sensitive threshold 
// added I2C wheel counter dr 

// added magnetic variation correction. dr with respect to TRUE NORTH. 


///////////////////////////////////////////////////////////////////////////// 
// Compiler settings for comms module test and for debugging with printf 
///////////////////////////////////////////////////////////////////////////// 
//set MODULE_TEST to 1 when running as a separate module 
//set MODULE_TEST to 0 when integrate with other files (using #use " ") 
tdefine MODULE_TEST 0 

//set DEBEGPRINT to 1 to enable all printf stdio for debugging 

//set DEBUGPRINT to 0 to disable printf when robot not connected to laptop 

tdefine DEBUGPRINT 0 


//set DEBUGSONAR to 1 to enable ONLY printf stdio for debugging SONAR STUFF 
//set DEBUGSONAR to 0 to disable printf 
tdefine DEBUGSONAR 1 

//set to enable printf statements to display wheel rotation sensors, 

//dr data, etc 

tdefine DEBUGDR 1 


////////////////////////////////////////////////////////////////////////////// 

//Declarations, global variables 

///////////////////////////////////////////////////////////////////////////// 

tclass auto 

void msDelay(unsigned int delay); 

///////////////////////////////////////////////////////////////////////////// 

// 2. Communications Module Code 

///////////////////////////////////////////////////////////////////////////// 

/* CommModule.c 

Charles Le, Zach Cole, and John Gamble 

This module enable the communication between the BL2000 and the remote 
control station (Java program) via UDP. There are total of 5 channels 
that UDP packets are sent to: 

4001 - MAN_CTRL_PORT packets (from Remote Control to BL2000) 

4002 - WYPNT_PORT - Way point packets (from Remote Control to BL2000) 

4003 - GPS_PORT - GPS data (from BL2000 to Remote Control) 

4004 - COMPASS_PORT - Compass data (BL2000 to Remote Control) 

4005 - ERROR_PORT - Error info (BL2000 to Remote Control) 

/ 

/* 

* Pick the predefined TCP/IP configuration for this sample. See 

* LIB\TCPIP\TCP_CONFIG.LIB for instructions on how to set the 

* configuration. 

*/ 

tdefine TCPCONFIG 1 


/* 
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* Define the number of socket buffers that will be allocated for 

* UDP sockets. We need five UDP sockets, so five socket buffer 

* will be used for MAX_UDP_SOCKET_BUFFERS. 

* FUTURE WORK: should only use one udp port for communication. 

* Concatinate all informations to one packet, and detokenize when it arrives 

* destination. 

*/ 

tdefine MAX_UDP_SOCKET_BUFFERS 5 


//The following udp ports are set 


tdefine 

MAN_CTRL_PORT 

4001 

tdefine 

WYPNT_PORT 

4002 

tdefine 

GPS_PORT 

4003 

tdefine 

COMPASS_PORT 

4004 

tdefine 

ERROR_PORT 

4005 


up to communicate with the Ctrl station 
//sent from Ctrl station (input port) 
//sent from Ctrl station (input port) 
//sent to Ctrl station (output port) 
//sent to Ctrl station (output port) 
//sent to Ctrl station (output port) 


//These 
tdefine 
tdefine 
tdefine 
tdefine 
tdefine 


are constants used in the Communication Module 
UDP_BUFF_SIZE 256 
TRUE 1 

FALSE 0 

PRESENT 1 

NOT_PRESENT 0 


//These constants specified the type of navigation data sent to the Ctrl module 
tdefine GPS_DATA 1 

tdefine COMPASS_DATA 2 

tdefine ERROR_DATA 3 


/* 

* If this is set to "0", we will accept a connection from anybody. 

* The first person to connect to us will complete the socket with 

* their IP address and port number, and the local socket will be 

* limited to that host only. 

•k 

* If it is set to all "255"s, we will receive all broadcast 

* packets instead. 

•k 

* THIS ADDRESS NEED TO BE IN THE SAME SUBNET WITH THE LOCAL IP ADDRESS 
*/ 

tdefine REMOTE_IP "192.168.4.37" 

//tdefine REMOTE_IP "255.255.255.255" /*broadcast*/ 


* End of configuration section * 


tmemmap xmem 
tuse "dcrtcp.lib" 
tuse "udp.lib" 

//This data structure describe the data structure of the communication 
//between the Communication Module and the Navigation Module 
typedef struct CommChanStruct 
{ 

int DataPresentFlag; 
char Buff[UDP_BUFF_STZE] ; 

} CommChan; 

int receive_packet(void) ; 
int receive_packet_from_port(int ) ; 
int send packet(int PortNumber, char* ); 
void ClearDataPresent(CommChan *) ; 
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void SetDataPresent(CommChan *) ; 

int CheckDataPresent(CommChan *); 

void ClearChannelBuffer(CommChan *); 

void InitUdpComm(void); 

int SendToControlStation(int ) ; 

void SendGpsToControlStation(void); 

void SendCompassToControlStation(void) ; 

void SendErrorToControlStation(void) ; 

void SendToCommModule(CommChan *, char *); 

char* GetMessageFromCommModule(CommChan *); 

//These are udp sockets that communicate with the Ctrl station 

static udp_Socket ManCtrlSock, WayPtSock, GpsSock, CompassSock, ErrorSock; 

//These are the communication channels between the Communication Module 
//and the Navigation Module 

static CommChan ManCtrlChan, WayPtChan,GpsChan, CompassChan, ErrorChan; 
char ErrString[200]; //for feedback to laptop control station 

////////////////////////////////////////////////////////////////////////////// 

// 3. IMU 

///////////////////////////////////////////////////////////////////////////// 

// serial buffer size 
tdefine FINBUFSIZE 255 
tdefine FOUTBUFSIZE 31 
// serial baud rate 
tdefine BAUD232 19200 
tdefine IMUHEADLIMIT 0.0175 
tdefine IMUROTLIMIT 0.010227 
char imuChar; 

char *xRot, *yRot, *zRot; // raw x, y, and z axis body rotation chars from IMU 
int imuZeroPoint; //nominal 511 for zero rotation 
float zRotation[7]; // zRot in float + or - 

// value in rad/s rotation rate 

of chassis 

long imuTimes[7]; //hold the times the IMU was recorded 
float imuHeading; 

int newimu; //boolean flag new IMU data avail 

int getimu (void) ; // reads IMU sentence and parses for z axis rotation 

rate 


////////////////////////////////////////////////////////////////////////////// 

// 4. I2C Compass 

///////////////////////////////////////////////////////////////////////////// 

tuse "I2C.LIB" 

//reads I2C compass sets global 1 byte integer heading 
int compass(char addrsByte, char registerByte) ; 

//PA6 SCL output, PA7 SDA output, PBO SCL input PB2 SDA input 
//Definition of Clock and Data Ports 

// Enable Digital out DIO 00 and DIO 01 digital outputs (sinking only) 

// Enable Digital DIO 2,03,04 outputs for motor direction signals 
tdefine DIGOUTCONFIG OxOOlF 

int heading; // integer 1 to 255 value current observed mag heading 

int headingLast; // last observed heading from compass 

int newCompass; // boolean flag set when new compass data is available 

// from I2C compass 

// heading commanded by CPU to maintain 
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int holdHeading; // boolean TRUE if heading hold engaged 

//MAGYAR Monterey 14 E as 1 byte value (14 deg * 255 bits/360 deg) 
tdefine MAGYAR 9.91667 

tdefine NORTH 255 // 1 byte value is 255 for 360 degrees 

// limit of allowed hdg error 2 * 1.4 degrees = 2.8 degree slop 

tdefine HEADINGSLOP 2 

//Proportional coefficient for PID 

tdefine KAPPA_P 12 

//Derivative coeff. for PID heading control feedback 
tdefine KAPPA_D 0.8 

tdefine KAPPA_I 0 


////////////////////////////////////////////////////////////////////////////// 

// 5. Waypoint 

///////////////////////////////////////////////////////////////////////////// 
typedef struct 
{ 

double lat; 
double Ion; 
char action; 

}WP; 

WP waypoints[13]; // 0th element is the Origin lat, long. 

// Then 1 to 12th elements 

are waypoints 

int current_wp_count; // a counter to keep track of waypoints 

void getwaypoints(void); // gets waypoints from GUI calls initNavO 

// to initialize 

nav 

void makeWayPts(void); // fills in array of type Position 

////////////////////////////////////////////////////////////////////////////// 

// 6. Navigation 

///////////////////////////////////////////////////////////////////////////// 

tuse "gps.lib" 

// POSITACCURACY IN cm 
tdefine POSITACCURACY 200 

// uses flat Earth approximation around robot's current position 
// Define polarity for robot, N hemisphere + and East hemisphere t 
//e^2 = f*(2-f) where f = 1/298.257223563 for WGS84 
const float esquared = .0066943800; 

const float a = 6378.13700; //radius Earth in km for WGS84 

//meridional radius of curvature and radius of curvature for prime vertical km 
float Rl, R2; 

GPSPosition OriginPos; // position of the flat Earth ORIGIN 
double initPos_lat, initPos_long; // lat, long in radians of the Origin 
char fakeGPSsentence[64]; //holds a fake GGA sentence to pass to GUI 
typedef struct{ 

long cm_N; 
long cm_E; 

} Position; 

Position currentPos; // declare a var of type Position for current robot pos. 
Position DRpos; // position based just on DR from wheel data 

Position waypointsPos[13]; // array of Position type to hold x, y 

// loctations of 

waypoints 

void getOriginGPSfromGUI(void) ; 
void initNav(void) ; 
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void makeFakeGPSpos(void) ; 

void navigation(int lastWaypt, int nextWaypt); 

////////////////////////////////////////////////////////////////////////////// 

// 7. DR 

///////////////////////////////////////////////////////////////////////////// 


int wheell, wheel2, wheels,• 
in Hz 


// observed values from wheel counter 


// 


optical wheel sensor produces 50 Hz in 1 rev 

// or 50 Hz in 2*pi radians 

float obsOmegal, obsOmega2, obsOmegaS; // observed radians/s wheel speeds 
float obsVx, obsVy; // observed robot velocity WRT robot's 

y, X axis cm/s 

float vE[3]; // velocities in Earth Frame in cm/s 

float vN[3]; 

float obsChassisOmega; // obsv'd robot chassis rotational speed rad/s 
float obsTheta; // obsv'd robot velocity vector direction 

// WRT robot's Y axis 


float obsV; // magnitude of robot's obsv'd velocity 

vector 

int newDR; // boolean flag set if newDR wheel 

speed data avail 

long DRtime; // time elapsed since last DR. Multipy 

velocities 

// times the DRtime to get 

position estimate 

//reads 1 wheel speed from counter 

int wheel(char addrsByte, char registerByte); 

// calculates velocity vector from wheel speeds in rad/s 

void calcVeloRobotFrame(float omegal, float omega2, float omega3); 


////////////////////////////////////////////////////////////////////////////// 

// 8. Obstacle Avoidance (IRs) 

///////////////////////////////////////////////////////////////////////////// 


float irRanges[6]; // 6 element array to hold the current IR voltage readings 
float irRangesOldl[6]; // array for the IR data previous to current data 
float irRanges01d2[6]; // array for IR data prior to Oldl (oldest data) 

float irRangesAvg[6]; // average values of last two IR data sets 

long watchDogTime; //time last OK motion. For detection stuck wheels 


tdefine TIMELIM 4000 
tdefine STUCKLIMIT 0.1 
tdefine IRSTOP_RNG 
tdefine IRCAPTURE_RNG 
tdefine CRAWLSPEED 
tdefine SLOWROTATESPEED 
int irCloseContact(void); 
IR_STOP_RNG 


1 . 6 
1.0 

.5 

3 

//tests irRanges[] for readings exceeding 


////////////////////////////////////////////////////////////////////////////// 

// 9. Sonar Sensor 

///////////////////////////////////////////////////////////////////////////// 


//serial C port to PIC 
tdefine SONARCONVERSION 1.05 
tdefine CINBUFSIZE 127 
tdefine COUTBUFSIZE 63 
tdefine CLOSERANGE 40 
tdefine MIDRANGE 90 
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tdefine NOCONTACT 250 

tdefine NUM_SONAR_AZIMUTHS 20 // 20 azimuths with 2 bytes per range reading 

char input_char; // serial char input 

int avoidObstacles; // boolean flag True of False to do obstacle avoidance 

float sonarRanges[24]; // array to hold range data in cm for 20 azimuths 

int newestSonar; //holds number of the newest sonar reporting range 

int closeContact; // boolean flag sonar contact close 

int objectAhead; // boolean flag obstacle ahead 

int clutterdArea; // boolean flag for behavior 

int getSonarRanges(void); 
int checkSonarRanges(void); 

long doRandSpin (void) ; // does random spin in place for 500 to 1500 

ms 

long spinDelay; // time in ms to delay while spinning in place 

////////////////////////////////////////////////////////////////////////////// 

// 10. Motion 

///////////////////////////////////////////////////////////////////////////// 

tdefine RADWHEEL 5.250 // radius in cm for motion in cm 

tdefine RADCHASSIS 22 // radius from bot's center to wheel cm 

tdefine FWD_DIR 0 

tdefine REV_DIR 1 

tdefine STOPVOLTS 0.00 

tdefine MAXVOLTS 4.10 

// max angular wheel speed 

// approx 220Hz on tach (with 50 division/1 rev optical targets) 

// or 27.6 radians /s 

//tdefine MAXOMEGA 13.80 WRONG VALUE 

tdefine MAXOMEGA 27.64 

//analog speed signals output 

tdefine mtrlChan 0 

tdefine mtr2Chan 1 

tdefine mtrSChan 2 

// channels for digital direction signals 
tdefine dirlChan 2 
tdefine dir2Chan 3 
tdefine dir3Chan 4 

// motion, hdg vars and function prototypes 

int robotStopped; //boolean flag TRUE if wheel counters go to zero 

int robotNotRotating; //boolean flag set TRUE if chassisOmega zero 

float mtrlSpd; //motor 1 analog voltage speed signal 0 to 4.096 (from BL2000) 

float mtr3Spd; // motor 2 analog speed signal 

float mtr2Spd; // motor 3 analog speed signal 

int dirl; // motor 1 direction. 

// boolean values 1 = reverse = REV_DIR, 0 = 

FWD_DIR 

int dir3; // motor 2 direction 

int dir2; // motor 3 direction 

float v; //robot's velocity vector magnitude, using scale 0 to 1.0 

float cmdVelocity; // remember original velocity to maintain 

float theta; // angle in degrees between Y axis (alligned with motor 1) 

// and vector 

float chassisOmega; //body or chassis rotation angular velocity rad/s 
// function prototypes 

int stopMotors(void); // stops motors. calls setMotorVolts 

int spinInPlace(void); // rotate in place 

int getVeloVect(void); //gets user input 

int solveMotorSpeeds(void); //solves three wheel speeds given v and theta 
// converts a wheel angular speed in rad/s to analog volts needed to output 
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float freqToVolts(float omegain); 

// sets analog output values to pins 

int setMotorVolts(float spdl, int dirl, float spd2, int dir2, float spd3,\ 
int dir3); 

int vector(void); //move robot in any direction calls setMotorVolts 


////////////////////////////////////////////////////////////////////////////// 

// 11. Control 

////////////////////////////////////////////////////////////////////////////// 

int manual_control_flag; // boolean flag indicates user has taken manual 

control 

void manual_control(void) ; 


////////////////////////////////////////////////////////////////////////////// 

// Shared functions // 

////////////////////////////////////////////////////////////////////////////// 

////////////////////////////////////////////////////////////////////////////// 

// Function: msDelay 

// Programmer: Kirk Volland 

// Input: desired time delay in ms 

// Output: Nil 

// Description: To delay the processor with the input time in ms, primarily 
// for serial and I2C communications. 

////////////////////////////////////////////////////////////////////////////// 

void msDelay(unsigned int delay) 

{ 

unsigned long done_time; 

done_time = MS_TIMER + delay; 
while ( (long) (MS_TIMER - done_time) < 0 ); 

} 


////////////////////////////////////////////////////////////////////////////// 
// END GLOBAL VARS DECLARATIONS, MACROS, AND EUNCTION PROTOTYPES 
// BEGIN EUNCTION DEEINITIONS 

////////////////////////////////////////////////////////////////////////////// 

/////////////////////////////////////////////////////////////////////////////// 

111. Communications 

n IIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIII! 
/ 

IIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIII n n n n n n n n! 
/ 

// Eunction: receive packet 
// Programmer: Charles Le 
// Input: None 

// Output: return 1 for a successful receive, and 0 if fail to send 
// Description: This program will check for way point input and manual control 
// input from the remote control station (Java Program) 

/////////////////////////////////////////////////////////////////////////////// 
/ 

int receive_packet(void) 

{ 


#GLOBAL_INIT 

{ 

memset(ManCtrlChan.Buff, 0, sizeof(ManCtrlChan.Buff)); 
memset(WayPtChan.Buff, 0, sizeof(WayPtChan.Buff) ) ; 

} 
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memset(ManCtrlChan.Buff, 0, sizeof(ManCtrlChan.Buff) ) ; 
memset(WayPtChan.Buff, 0, sizeof(WayPtChan.Buff)); 

udp_recv(SWayPtSock, WayPtChan.Buff, sizeof(WayPtChan.Buff)) ; 

#if DEBUGPRINT 

printf("\n WayPtSock-> %s\n" , WayPtChan.Buff) ; 

#endif 

// receive the packet 

udp_recv(SManCtrlSock, ManCtrlChan.Buff, sizeof(ManCtrlChan.Buff)); 

#if DEBUGPRINT 

//printf("\n ManCtrlSock-> %s\n",ManCtrlChan.Buff); 

#endif 

tcp_tick(NULL) ; 
return 1; 

} 

/////////////////////////////////////////////////////////////////////////////// 

/ 

// Function: receive packet from port 
// Programmer: Charles Le 
// Input: None 

// Output: return 1 for a successful receive, and 0 if fail to send 
// Description: This program will receive udp packet from the specified port 
/////////////////////////////////////////////////////////////////////////////// 
/ 

int receive_packet_from_port(int PortNumber) 

{ 

int retval; 

#GLOBAL_INIT 

{ 

memset(ManCtrlChan.Buff, 0, sizeof(ManCtrlChan.Buff)); 
memset(WayPtChan.Buff, 0, sizeof(WayPtChan.Buff)); 

} 


memset(ManCtrlChan.Buff, 0, sizeof(ManCtrlChan.Buff)); 
memset(WayPtChan.Buff, 0, sizeof(WayPtChan.Buff)); 

switch(PortNumber) 

{ 

case WYPNT_PORT: 

{ 

retval = udp_recv(SWayPtSock, WayPtChan.Buff, sizeof(WayPtChan.Buff)); 
if (retval < 0) { 

sock_close(SWayPtSock) ; 

if(!udp_open(SWayPtSock, PortNumber, /*resolve(REM0TE_IP)*/ -1, \ 

PortNumber, NULL)) 

{ 

exit(0); 

} 

} 

else 

{ 

SetDataPresent(SWayPtChan); 

} 

tcp_tick(NULL); 

} 

case MAN_CTRL_PORT: 

{ 


154 



retval = udp_recv(SManCtrlSock, ManCtrlChan.Buff, \ 
sizeof(ManCtrlChan.Buff)); 

if (retval < 0) { 

sock_close(SManCtrlSock) ; 

if(!udp_open(SManCtrlSock, PortNumber, /*resolve(REMOTE_IP)*/ -1, 

PortNumber, NULL)) 

{ 

exit(0); 

} 

} 

else 

{ 

SetDataPresent(SManCtrlChan) ; 

} 

tcp_tick(NULL) ; 

} 

default: 

{ 

//error incorrect port number 
return 0; 


} 

tcp_tick(NULL); 
return 1; 

} 

/////////////////////////////////////////////////////////////////////////////// 

/ 

// Function: send_packet 
// Programmer: Charles Le 
// Input: PortNumber, messagein 

// Output: Return 1 for a successful sending and 0 for fail to send 

// Description: The function will send the input message to the input udp port 

/////////////////////////////////////////////////////////////////////////////// 

/ 

int send_packet(int PortNumber, char* messagein) 

{ 

auto int length, retval; 
udp_Socket *sock; 

length = strlen(messagein) + 1; 

switch(PortNumber) 

{ 

case GPS_P0RT: 

{ 

retval = udp_send(SGpsSock, messagein, length); 

if (retval < 0) { 

#if DEBUGPRINT 

printf ("Error sending datagram! Closing and reopening 

socket. . .\n" ) ; 

#endif 

sock_close(SGpsSock); 

if(!udp_open(SGpsSock, PortNumber, /*resolve(REM0TE_IP)*/ -1, \ 

PortNumber, NULL)) 

{ 

#if DEBUGPRINT 

printf("udp_open failed!\n"); 

#endif 
exit (0); 
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} 

} 


} 

case COMPASS_PORT: 

{ 

retval = udp_send(SCompassSock, messagein, length); 

if (retval < 0) { 

#if DEBUGPRINT 

printf("Error sending datagram! Closing and reopening socket...\n"); 
#endif 

sock_close(SCompassSock) ; 

if(!udp_open(SCompassSock, PortNumber, /*resolve(REMOTE_IP)*/ -1, 

PortNumber, NULL)) 

{ 

#if DEBUGPRINT 

printf("udp_open failed!\n"); 

#endif 
exit (0); 

} 

} 

} 

case ERR0R_P0RT: 

{ 

retval = udp_send(&ErrorSock, messagein, length); 

if (retval < 0) { 

#if DEBUGPRINT 

printf("Error sending datagram! Closing and reopening socket...\n"); 
#endif 

sock_close(SErrorSock) ; 

if(!udp_open(SErrorSock, PortNumber, /*resolve(REM0TE_IP)*/ -1,\ 
PortNumber, NULL)) 

{ 

#if DEBUGPRINT 

printf("udp_open failed!\n"); 

#endif 
exit(0); 

} 

} 

} 

default: 

{ 

//error incorrect port number 
return 0; 


} 

tcp_tick(NULL); 
return 1; 

} 

/////////////////////////////////////////////////////////////////////////////// 

/ 

// Function: ClearDataPresent 
// Programmer: Charles Le 

// Input: DataChannel 
/ / Output: None 

// Description: This function will set DataPresentFlag to 0 

/////////////////////////////////////////////////////////////////////////////// 


void ClearDataPresent(CommChan *Channel) 

{ 
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Channel->DataPresentFlag = FALSE; 


} 

/////////////////////////////////////////////////////////////////////////////// 

/ 

// Function: SetDataPresent 
// Programmer: Charles Le 

// Input: DataChannel 
// Output: None 

// Description: This function will set DataPresentFlag to 1 

/////////////////////////////////////////////////////////////////////////////// 

/ 

void SetDataPresent(CommChan *Channel) 

{ 

Channel->DataPresentFlag = TRUE; 

} 

/////////////////////////////////////////////////////////////////////////////// 

/ 

// Function: SendToCommModule 
// Programmer: Charles Le 

// Input: DataChannel 
// Output: None 

// Description: This function will set DataPresentFlag to 1 

/////////////////////////////////////////////////////////////////////////////// 

/ 

void SendToCommModule(CommChan *Channel, char *Message) 

{ 

SetDataPresent(Channel); 
strcpy(Channel->Buff,Message) ; 

} 

/////////////////////////////////////////////////////////////////////////////// 

/ 

// Function: CheckDataPresent 
// Programmer: Charles Le 

// Input: DataChannel 
// Output: None 

// Description: This function will set return DataPresentFlag 

/////////////////////////////////////////////////////////////////////////////// 

/ 

int CheckDataPresent(CommChan *Channel) 

{ 

return Channel->DataPresentFlag; 

} 

/////////////////////////////////////////////////////////////////////////////// 

/ 

// Function: ClearChannelBuffer 
// Programmer: Charles Le 

// Input: DataChannel 
// Output: None 

// Description: This function will clear channel buffer to 0 

/////////////////////////////////////////////////////////////////////////////// 

/ 

void ClearChannelBuffer(CommChan *Channel) 

{ 

memset(Channel->Buff, 0, sizeof (Channel->Buff)); 

} 

/////////////////////////////////////////////////////////////////////////////// 

/ 

// Function: InitUdpComm 
// Programmer: Charles Le 
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// Input: None 
// Output: None 

// Description: This function will initialize buffers and set upd port for 

// communication between BL2000 to the remote control. These ports 

will 

// be set up: 4001, 4002, 4003, 4004, 4005 

/////////////////////////////////////////////////////////////////////////////// 

/ 

void InitUdpComm(void) 

{ 

sock_init (); 

#if DEBUGPRINT 

printf("Opening UDP socketXn"); 

#endif 

ClearDataPresent(SManCtrlChan) ; 

ClearDataPresent(SWayPtChan) ; 

ClearDataPresent(SGpsChan) ; 

ClearDataPresent(SCompassChan) ; 

ClearDataPresent(SErrorChan) ; 


ClearChannelBuffer(SManCtrlChan) ; 
ClearChannelBuffer(SWayPtChan) ; 
ClearChannelBuffer(&GpsChan) ; 
ClearChannelBuffer(&CompassChan) ; 
ClearChannelBuffer(SErrorChan) ; 


sock_mode( &ManCtrlSock, UDP_MODE_NOCHK); 

if(!udp_open(SManCtrlSock, MAN_CTRL_PORT, /*resolve(REMOTE_IP)*/-l, \ 

MAN_CTRL_PORT, NULL)) { 

#if DEBUGPRINT 

printf("udp_open failed!\n"); 

#endif 
exit (0); 

} 


sock_mode( SWayPtSock, UDP_MODE_NOCHK); 

if(!udp_open(SWayPtSock, WYPNT_PORT, /*resolve(REMOTE_IP)*/-l, \ 

WYPNT_PORT, NULL)) { 

#if DEBUGPRINT 

printf("udp_open failed!\n"); 

#endif 
exit (0); 

} 


sock_mode( SGpsSock, UDP_MODE_NOCHK); 

if(!udp_open(SGpsSock, GPS_PORT, /*resolve(REM0TE_IP)*/-l, GPS_P0RT, NULL)) 
#if DEBUGPRINT 

printf("udp_open failed!\n"); 

#endif 
exit (0); 

} 


sock_mode( &CompassSock, UDP_MODE_NOCHK); 

if(!udp_open(SCompassSock, COMPASS_PORT, /*resolve(REMOTE_IP)*/-l, \ 

COMPASS_PORT, NULL)) { 

#if DEBUGPRINT 

printf("udp_open failed!\n"); 
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#endif 
exit (0); 


sock_mode( &ErrorSock, UDP_MODE_NOCHK); 

if(!udp_open(SErrorSock, ERR0R_P0RT, /*resolve(REM0TE_IP)*/-l, \ 

ERR0R_P0RT, NULL)) { 

#if DEBUGPRINT 

printf("udp_open failed!\n" ) ; 

#endif 
exit (0); 

} 

} //end of function 

/////////////////////////////////////////////////////////////////////////////// 

/ 

// Function: GetMessageFromCommModule 
// Programmer: Charles Le 

// Input: CommChan 

// Output: return the command from the Remote Controler and clear the present 
// flag 

// Description: This function will return the command from the Remote Controler 
// and clear the present flag 

/////////////////////////////////////////////////////////////////////////////// 
/ 

char* GetMessageFromCommModule(CommChan *Channel) 

{ 

ClearDataPresent(Channel); 
return Channel->Buff; 

} 

/////////////////////////////////////////////////////////////////////////////// 

/ 

// Function: SendGpsToControlStation 
// Programmer: Charles Le 

// Input: None 
// Output: None 

// Description: This function will send the Gps data to the Ctrl station 

/////////////////////////////////////////////////////////////////////////////// 

/ 

void SendGpsToControlStation(void) 

{ 

tcp_tick(NULL) ; 

if (CheckDataPresent(SGpsChan)) 

{ 

send_packet(GPS_PORT,GpsChan.Buff) ; 
tcp_tick(NULL) ; 

ClearDataPresent(SGpsChan) ; 

} 

} 

/////////////////////////////////////////////////////////////////////////////// 

/ 

// Function: SendCompassToControlStation 
// Programmer: Charles Le 

// Input: None 
// Output: None 

// Description: This function will send the compass data to the Ctrl station 

/////////////////////////////////////////////////////////////////////////////// 

/ 

void SendCompassToControlStation(void) 

{ 

tcp_tick(NULL); 

if (CheckDataPresent(SCompassChan)) 
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{ 

send packet(COMPASS_PORT,CompassChan.Buff) ; 
tcp_tick(NULL) ; 

ClearDataPresent(SCompassChan) ; 

} 

} 

/////////////////////////////////////////////////////////////////////////////// 

/ 

// Function: SendErrorToControlStation 
// Programmer: Charles Le 

// Input: None 
// Output: None 

// Description: This function will send the error data to the Ctrl station 

/////////////////////////////////////////////////////////////////////////////// 

/ 

void SendErrorToControlStation(void) 

{ 

tcp_tick(NULL) ; 

if (CheckDataPresent(SErrorChan)) 

{ 

send_packet(ERROR_PORT, ErrorChan.Buff) ; 
tcp_tick(NULL) ; 

ClearDataPresent(SErrorChan) ; 

} 


////////////////////////////////////////////////////////////////////////////// 

//3. IMU 

////////////////////////////////////////////////////////////////////////////// 

/////////////////////////////////////////////////////////////////////////////// 

/ 

// Function: getimu 
// Programmer: Kirk Volland 

// Input: None 

// Output: int 0 success, 1 failure 

// Description: This function does RS232 communications with IMU. Reads in 
// chars from serial port until the end of line carriage return detected. 

// Parses string for rotation rates. Saves values to globals. 

/////////////////////////////////////////////////////////////////////////////// 

/ 

int getimu(void) 

{ 

int i; 

char imuStr[128]; 

for(i =6; i > 0; i—) //for averaging the rotation rate 

{ //shift older data to end of array 

zRotation[i] = zRotation[i-1]; //array element 0 is the newest 
imuTimes[i] = imuTimes[i-1]; 


i = 0; 

strcpy(imuStr, ""); //null out any contents 

imuStr[i] = imuChar; //record the leading # 

i++; 

//get an IMU sentence of rotation values, and linear accelerations 
while ( (imuChar = serFgetc() ) !=-!&& imuChar != '\r' ) 

{ 

//save chars to array if they are not -1 or the ending 
//rawImuLog[logindex + i] = imuChar; //null it out 

imuStr[i] = imuChar; 
i + + ; 

if( i > 127) 
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i = 0; //reset i so less than max element value of array 


} 

//find the z-rotaion and use ASCII to float to store it's value 

//parse imuStr 

xRot = strtok(imuStr, 

yRot = strtok(NULL, 

zRot = strtok(NULL, 

imuTimes[0] = MS_TIMER; 

if(zRot == NULL) 

{ 

zRotation[0] = 999.9; //error non-value 
return 1; 

} 

else 

{ //subtract 511 from value 

//conversion 300deg/1024 bits * (2* PI /360 degrees) 

//rotation rate defined positive for Counter clockwise body rotation 
//NOTE, nominal 511 value changed to 512 zero reference based on 
// observations of installed IMU when robot stopped. zRotation in rad/s 
zRotation[0] = (-1* rad ( (atof(zRot) - imuZeroPoint) * (float) 300/1024) 

return 0; 

} 


////////////////////////////////////////////////////////////////////////////// 

//4. compass (I2C) 

////////////////////////////////////////////////////////////////////////////// 

////////////////////////////////////////////////////////////////////////////// 
// Function: compass 
// Programmer: Kirk Volland 

// Input: char I2C address and char register to read 
// Output: int mag. compass heading 1 to 255 

// Description: This function communicates with the digital compass using I2C 
// protocol and converts the readout from the compass into int newHeading. 

// returns int value. 

////////////////////////////////////////////////////////////////////////////// 

nodebug int compass(char addrsByte, char registerByte) 

{ 

char cmpd; 
int err; 

int newHeading, i2cTries; 

if (err = i2c_startw_tx() ) 

{ 

i2c_stop_tx() ; 

return -10 + err; // Error. Return clock stretching too long 

} 

if (err=i2c_wr_wait(addrsByte)) 

{ 

i2c_stop_tx() ; 

return -20+err; // Return no ack on slave (retried) 

} 

if (err=i2c_write_char(registerByte)) 

{ 

i2c_stop_tx(); 

return -30+err; // Return no ack on data register 01 

} 

if (err=i2c_startw_tx()) 

{ 
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i2c_stop_tx() ; 

return -40+err; // Return too long stretch on read 

} 

if (err=i2c_wr_wait(addrsByte +1)) 

{ 

i2c_stop_tx() ; 

return -50+err; // Send read to slave - no ack (retried) return -5 


if ( err=i2c_read_char(Scmpd)) 

{ 

i2c_stop_tx(); 

return -60+err; 


else 

{ 


for ( i2cTries = 0; i2cTries < 4; i2cTries ++) 


if (err = i2c_send_ack()) 

{ 

// nothing. try again 

} 

else // no error. OK reception of ack from 

{ 

i2c_stop_tx(); 

headingLast = heading; 

heading = (int) (cmpd + MAGVAR); 

return 0; // normal exit 


} 


slave 


i2c_stop_tx() ; 

headingLast = heading; 

heading = (int) (cmpd + MAGVAR); 

return -70 + err; // not reev'd ack from slave 

} 

} 


////////////////////////////////////////////////////////////////////////////// 

//5. Waypoint 

////////////////////////////////////////////////////////////////////////////// 

////////////////////////////////////////////////////////////////////////////// 

// Function: getwaypoints 
// Programmer: Alex, Kirk 
// Input: Nil 
// Output: Nil 

// Description: This function put the sets of waypoints that was sent from 
comms 

// module into global data (array of structure for waypoints). NOTE, the GUI's 
// polarity is Western hemisphere +. Code assumes N and W hemispheres. 

// Add UPDATED CODE for others LATER. 

// Obstacle avoidance has 3 waypoints (index 0,1,2) reserved for nav, but they 
// are not used in the WINTER2007 avoidance code. 
////////////////////////////////////////////////////////////////////////////// 
void getwaypoints(void) 

{ 

char* WayPtStr; 

char TempLat[20], TempLong[20], TempAction[5]; 
int Startindex; 
char DummyStr[100] ; 
char tempBuf[20]; 
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StartIndex=0; 

strcpy(TempLat,"0.01"); //initialize templat to not 0.0 
memset(TempLat,0,sizeof(TempLat)) ; 
memset(TempLong,0,sizeof(TempLong) ) ; 
memset(TempAction,0,sizeof(TempAction)); 
WaYPtStr=GetMessageFromCommModule(SWayPtChan); 

//WaYPtStr=WayPtChan.Buff; 

#if DEBUGPRINT 

printf("WaYPtStr=%s\n",WayPtStr) ; 

#endif 


WayPtStr=WayPtStr+l; 

for (Startlndex= 0; Startlndex<= 12; Startlndex++) 

{ 

//Separate waypoint tokens 
if (Startlndex==0) 

strcpy (TempLat,strtok(WayPtStr, " " ) ) ; 

else 

strcpy(TempLat,strtok(NULL, " " ) ) ; 

strcpy (TempLong,strtok(NULL, " " ) ) ; 
strcpy (TempAction,strtok(NULL, " ") ) ; 

//copy tokens to the WayPoint Structures 
// assume Northern Hemis. lat's 

waypoints[Startindex] .lat= (double) atof (TempLat); 

//assume Western Hemisphere long. 

waypoints[Startindex] .lon= (double) atof (TempLong); 
waypoints[Startindex].action=TempAction[0]; 

#if DEBUGPRINT 

printf("waypoints[%d].lat=%lf\n",Startindex,waypoints[Startindex].lat); 
printf("waypoints[%d].lon=%lf\n",Startindex,waypoints[Startindex].Ion); 
printf("waypoints[%d].action=%c\n",Startindex,waypoints[Startindex].actio 

r 

#endif 

// send acknowledgement on Error channel back to laptop 
/* 

sprintf(tempBuf, "waypoint[%d] Lat:%f Long:%f\n", Startindex,/ 
waypoints[Startindex].lat , waypoints[Startindex].Ion); 
strcat(ErrString, tempBuf); 

*/ 


} 

////////////////////////////////////////////////////////////////////////////// 
// Function: makeWayPts 
// Programmer: Kirk 
// Input: Nil 
// Output: Nil 

// Description: Fills in array of type Position with the distances in cm North 
// and East of the Origin. 

////////////////////////////////////////////////////////////////////////////// 

void makeWayPts(void) 

{ 

int i; 

double tempLat, tempLong; 

for(i = 1; i < 13; i++) 

{ 
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if(waypoints[i].lat != 0) 

{ 

tempLat = waypoints[i].lat; 

// make radians for math in flat Earth 
tempLat = (double) rad(tempLat); 
tempLong = waypoints[i].Ion; 

// make radians for math in flat Earth 
tempLong = (double) rad(tempLong); 

// record into array of structs using flat Earth approx's 

global 

// R1 radius and R2 radius 
// multiply Radii in km by 1E5 to get radii in cm 

waypointsPos[i].cm_N = (long) R1 * 1E5* (tempLat - 

initPos_lat); 

waypointsPos[i].cm_E = (long) R2 * 1E5* cos(initPos_lat) * 

\ 

(initPos_long- tempLong ); 

#if DEBUGPRINT 

printf("tempLat %.81f initPos_lat %.81f\n", tempLat, initPos_lat); 
printf("tempLong %.81f initPos_long %.81f\n", tempLong, initPos_long); 
printf("waypointsPos[%d].cm_N =%ld\n", i,waypointsPos[i].cm_N); 
printf("waypointsPos[%d].cm_E =%ld\n",i,waypointsPos[i].cm_E); 

#endif 

} 

else 

{ 

waypointsPos[i].cm_N = 0; 

waypointsPos[i].cm_E = 0; //default to Origin if bogus 

} 


////////////////////////////////////////////////////////////////////////////// 

//6. Navigation 

////////////////////////////////////////////////////////////////////////////// 


////////////////////////////////////////////////////////////////////////////// 
// Function: getOriginGPSfromGUI 
// Programmer: Kirk 
// Input: Nil 
// Output: Nil 

// Description: Sets the lat and long of the GPSPosition struct for the 
// Origin's lat, long from waypoint 0. 

////////////////////////////////////////////////////////////////////////////// 

void getOriginGPSfromGUI(void) 

{ 


initPos_lat = waypoints[0].lat; //save the float value as degrees from 

GUI 

//toss out decimal portion, keep integer degrees 
OriginPos.lat_degrees = (int) waypoints[0].lat; 
initPos_long = waypoints[0].Ion; 

OriginPos.lon_degrees = (int) waypoints[0].Ion; 

//make degrees minutes, decimal minutes for GPS style format 
//get the fraction remainder left over in floating pt form 
// and multiply by 60 to make decimal minutes 

OriginPos.lat_minutes = (waypoints[0].lat - OriginPos.lat_degrees) *60; 
OriginPos.lon_minutes = (waypoints[0].Ion - OriginPos.lon_degrees)*60; 
OriginPos.lat_direction = 'N'; //assume N and West hemispheres 
OriginPos.lon_direction = 'W'; 

} 


164 



////////////////////////////////////////////////////////////////////////////// 

// Function: initNav 
// Programmer: Kirk 
// Input: Nil 
// Output: Nil 

// Description: Solves for some global variables Rl, R2, and the Origin's 
// lat and long in radians to speed up calcs in the flat Earth approx, math. 
////////////////////////////////////////////////////////////////////////////// 
void initNav(void) 

{ 

int i; 

getOriginGPSfromGUI(); // input the GPS position for the Origin 

// set the Origin's GPS lat, long 

//solve for the Origin's lat, long in radians for flat Earth nav approx. 

if(OriginPos.lat_direction=='S') // get the sign convention correct. 

initPos_lat= 0 - initPos_lat; //negative values in Southern 

hemisphere 

// make radians for math in flat Earth 
initPos_lat = (double) rad(initPos_lat); 

if(OriginPos.lon_direction=='E') 

initPos_long= 0 - initPos_long; //negative values for East hemisphere 
// make radians for math in flat Earth 
initPos_long = (double) rad(initPos_long); 

//find Rl and R2, radii of curvature in km 
//esquared is measure of flatness from WGS84 baseline 

Rl = a*(l - esquared) / pow ( (1- esquared* pow (sin (initPos_lat) , 2) ), 

1.5) ; 

R2 = a/(sqrt(1- esquared* pow(sin(initPos_lat),2))); 

DRpos.cm_N = 0; 

DRpos.cm_E =0; // put the robot at the Origin 
for(i = 0; i< 3; i++) //init the dr, too 
{ 

vE[i] = 0; 

vN[i] = 0; // zero initial speed 

} 

} 

////////////////////////////////////////////////////////////////////////////// 

// Function: makeFakeGPSpos 
// Programmer: Kirk 
// Input: Nil 
// Output: Nil 

// Description: Creates a sentence that simulates GGA sentence that the GUI 
// expects. currentLat and currentLong are found from distance N. and dist 
// E. of the Origin summed with the lat, long of the Origin. Going Eastward 
// subtracts longitude from Origin's longitude. 
////////////////////////////////////////////////////////////////////////////// 
nodebug void makeFakeGPSpos(void) 

{ 

double currentLat, currentLong; 
int latDeg, longDeg; 
float flatDeg, flongDeg; 
float latMin, longMin; 

//find "lat" and "long" from the distance in cm N and E. of the Origin of 
flat Earth 

//distN is in cm so divide it by 1E5 for km 

currentLat = DRpos.cm_N / (Rl* 1E5) + initPos_lat; 

// lat and long in radians 

currentLong = initPos_long - DRpos.cm_E / (R2* 1E5 * 
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//convert to degrees 
flatDeg = deg(currentLat); 
flongDeg = deg(currentLong); 

//break into degrees and decimal minutes 

latDeg = (int) flatDeg; // get the integer degree values 
longDeg = (int) flongDeg; 

latMin = (flatDeg - latDeg)*60; //get the fraction remainder left over 
in 

//floating pt form 

longMin = (flongDeg - longDeg)*60; // multiply by 60 to make decimal 

minutes 

//fake time? in fake NMEA sentence 
sprintf(fakeGPSsentence, \ 

"GPGGA, 223003.8,%02d%09.6f,N,%03d%09.6f,W,l,05,l.l,,\n", latDeg, latMin, \ 
longDeg, longMin); 

} 

////////////////////////////////////////////////////////////////////////////// 

// Function: navigation 
// Programmer: Kirk Volland 

// Input: integer last waypoint number, integer next waypoint number for 
// array waypointsPos[] 

// Output: Nil 

// Description: Solves for mag heading needed to move from current position 
// (in cm_N and cm_E of Origin) to the next waypoint. 
////////////////////////////////////////////////////////////////////////////// 

nodebug void navigation(int lastWaypt, int nextWaypt) 

{ 


long dN, dE; //difference (delta) North centimetrs 

//dN = waypoint N_cm - current N_cm 
//dE = waypoint E_cm - current E_cm 

double range; //range in cm to wypt from current position 

float gamma, course; 

int heading_diff; 

//find delta North (dN) and delta East (dE) 
dN = waypointsPos[nextWaypt].cm_N - DRpos.cm_N; 
dE = waypointsPos[nextWaypt].cm_E - DRpos.cm_E; 

range = sqrt( pow(dN, 2) + pow(dE, 2) ); //pythag. theorem 

theta = rad(300); 


////////////////////////////////////////////////////////////////////////////// 

111. DR 

!IIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIII! 
!IIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIII! 

II Function: wheel 
// Programmer: Kirk 

// Input: address byte of I2C device, register 0x01, 0x02, or 0x03 memory 
// to read wheel angular speed in Hz. 

// Output: integer wheel speed in Hz or negative integer if I2C error 
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// Description: This function uses I2C lib to call device with addrsByte to 
// read the value of the memory 0x01 for wheel 1 speed, 0x02 for wheel 2 speed, 
//or 0x03 for wheel 3 speed. Returns wheel speed in Hz. 
////////////////////////////////////////////////////////////////////////////// 
nodebug int wheel(char addrsByte, char registerByte) 

{ 

char cmpd; 
int err; 

int wheelFrq, i2cTries; 

if (err = i2c_startw_tx() ) 

{ 

i2c_stop_tx() ; 

return -10 + err; // Error. Return clock stretching too long 

} 

if (err=i2c_wr_wait(addrsByte)) 

{ 

i2c_stop_tx() ; 

return -20+err; // Return no ack on slave (retried) 

} 

if (err=i2c_write_char(registerByte)) 

{ 

i2c_stop_tx(); 

return -30+err; // Return no ack on data register 01 

} 

//i2c_Delay(10) ; 
if (err=i2c_startw_tx()) 

{ 

i2c_stop_tx() ; 

return -40+err; // Return too long stretch on read 

} 

if (err=i2c_wr_wait(addrsByte +1)) 

{ 

i2c_stop_tx(); 

return -SOterr; // Send read to slave - no ack (retried) return -5 

} 

if ( err=i2c_read_char(&cmpd)) 

{ 

i2c_stop_tx() ; 

return -60+err; 

} 

else 

{ 

for ( i2cTries = 0; i2cTries < 4; i2cTries ++) 

{ 

if (err = i2c_send_ack()) 

{ 

// nothing. try again 

} 

else // no error. OK reception of ack from slave 

{ 

i2c_stop_tx(); 

wheelFrq = (int) (cmpd); 

return wheelFrq; // normal exit 

} 

} 

i2c_stop_tx() ; 

wheelFrq = (int)(cmpd); //freq is 2 times the reported 
return wheelFrq; // not reev'd ack from slave 

} 
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////////////////////////////////////////////////////////////////////////////// 
// Function: calcVeloRobotFrame 
// Programmer: Kirk 

// Input: float omegal, omega2, omegaS in rad/s 
// Output: Nil 

// Description: Finds obsv'd speeds along robot's y and x axis from wheel speed 
// data. Finds direction of observed motion theta WRT to y axis. Solves 
// magnitude of velocity vector and robot chassis rotation. 
////////////////////////////////////////////////////////////////////////////// 
nodebug void calcVeloRobotFrame(float omegal, float omegal, float omegaS) 

{ 

obsVx = RADWHEEL/3 * (-2*omegal + omegal + omegaS); 

// omegal perpendicular, no contribution to y axis motion 
obsVy = RADWHEEL/(sqrt(3)) * (omega3 - omegal); 
obsChassisOmega = RADWHEEL/3 * (omegal + omegal + omegal); 

// find magnitude of v 

obsV = sqrt(pow(obsVx,2) + pow(obsVy,2)); 

// find obsv'd theta (velocity vector WRT robot's y axis) 
if(fabs(obsVx) >0 && fabs(obsVy) > 0) 
obsTheta = atanl(obsVx,obsVy); 
else 

obsTheta = 0; 


} 

////////////////////////////////////////////////////////////////////////////// 
// Function: calcVeloEarthFrame 
// Programmer: Kirk 
// Input: Nil 
// Output: Nil 

// Description: Takes robot velocity WRT robot's axes 
// and translates it into Earth Frame using global var heading. 
////////////////////////////////////////////////////////////////////////////// 
nodebug void calcVeloEarthFrame(void) 

{ 

float gamma; // sum of heading and 

// observed velocity vector direction obsTheta 
//save 2nd oldest reading as the 3rd oldest 
vE [ 2 ] = vE [ 1 ] ; 
vN[2] = vN[1]; 

//save newest reading as the 2nd oldest 
vE [ 1 ] = vE [ 0 ] ; 
vN[1] = vN[0]; 


//update the newest readings in array element 0 

//heading is 0 to 255 1 byte value that corresponds to angle 0 to 2*PI 
gamma =( (float) (2*PI/255 * heading) )+ obsTheta; 

gamma = fmod(gamma, (2*PT)); // don't want values greater than 2*PI 
//test if gamma is in quadrant I, II, III or IV 
if(gamma < PI/2) 

{ 

vE[0] = obsV * sin(gamma); //quadrant I 
vN[0] = obsV * cos(gamma) ; 


else 

{ 

if (gamma < PI) 

{ 

vE[0] = obsV * sin (PI - gamma); // gamma > PI/2 but less than PI 
vN[0] = - obsV * cos (PI ~ gamma); // quadrant II 
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} 

else 

{ 

if(gamma < 3*PI/2) 

{ 

vE [ 0] = - obsV * sin(gamma- PI); //quadrant III 

vN[0] = - obsV * cos(gamma- PI); 

} 

else 

{ 

vE [ 0] = - obsV * sin(2*PI - gamma); //quadrant IV 

vN[0] = obsV * cos(2*PI - gamma); 


} 


////////////////////////////////////////////////////////////////////////////// 

// Function: dr 
// Programmer: Kirlc 
// Input: Nil 
// Output: Nil 

// Description: Takes robot velocity in Earth Frame and multiplies by time 
// since last DR to get the estimated position in N. and E components. Adds 
// N. and E. components to last position to get currentDRposition. 
////////////////////////////////////////////////////////////////////////////// 
nodebug void dr(void) 

{ 

//multipy speed times time in ms since last DR update 
float avgVN, avgVE; 

int i; 
avgVN = 0; 
avgVE = 0; 

//make average velocities over the last three readings 
for(i= 0; i< 3; i++) 

{ 

avgVN = vN[i] + avgVN; 
avgVE = vE[i] + avgVE; 

} 

//avgVN = avgVN/3; 

//avgVE = avgVE/3; 
avgVN = vN[0]; 
avgVE = vE[0]; 

DRpos.cm_N = DRpos.cm_N + (long) (avgVN * (float) (MS_TIMER - 
DRtime)/lOOO) ; 

DRpos.cm_E = DRpos.cm_E + (long) (avgVE * (float) (MS_TIMER - DRtime)/1000); 

DRtime = MS_TIMER; //save current time as "start" time for next round of DR 
#if DEBUGDR 

printf("DR posit N %ld cm E %ld cm\n", DRpos.cm_N, DRpos.cm_E); 

#endif 


////////////////////////////////////////////////////////////////////////////// 

//8. Collision Advoidance 

////////////////////////////////////////////////////////////////////////////// 

////////////////////////////////////////////////////////////////////////////// 
// Function: doRandSpin 
// Programmer: Kirk Volland 
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// Input: Nil 

// Output: long integer value in ms to delay as motors make robot spin 
// Description: Calls randNum for value 0 to 1.0 to make a time in milisec 
// between 900 and 1900 ms. Sets globals and returns the time delay back to 
// main() loop. 

////////////////////////////////////////////////////////////////////////////// 

long doRandSpin(void) 

{ 

float randNum; // a float value for coefficent in range 0.0 <= 

randNum < 1.0 

long randTurnTime; // time in ms. want values 500 to 1500 ms 
int dir; // random direction 

randNum = rand(); // find random value from 0 to 1.0 

// random long value from 900 to 900 +1000 = 1500 ms 
randTurnTime = (long) (900 + randNum *1000); 

dir = (int) randTurnTime % 2; 
chassisOmega = SLOWROTATESPEED; 

spinInPlace(); 
return randTurnTime; 

} 


////////////////////////////////////////////////////////////////////////////// 

// Function: getIRVoltsO 
// Programmer: Kirk Volland 
// Input: Nil 
// Output: Nil 

// Description: Gets analog voltage inputs from the IR sensors. Stores values 
// in the irRanges global array. Copies last round of readings to 
irRangesOldl. 

// Copies round before the last round to irRanges01d2. 

// 

////////////////////////////////////////////////////////////////////////////// 

void getIRVolts(void) 

{ 

int irNum; 

//record irRangesOldl to irRanges01d2 
for(irNum = 0; irNum < 6; irNum ++) 
irRangesOldl[irNum] = irRangesOldl[irNum]; 

//record last round of IR ranges before overwriting them 
for(irNum = 0; irNum < 6; irNum ++) 
irRangesOldl[irNum] = irRanges[irNum]; 

//sample 6 IR sensors 

//start with zero and count up to 5 for A to D on channels 0 to 5 
//newest IR data 

for(irNum = 0; irNum < 6; irNum++) 
irRanges[irNum] = anaInVolts(irNum, 3); 

//average last three readings 

for(irNum = 0; irNum < 6; irNum++) 

irRangesAvg[irNum] = (irRanges[irNum] + irRangesOldl[irNum])/2; 

} 


////////////////////////////////////////////////////////////////////////////// 
// Function: irCloseContact 
// Programmer: Kirk 
// Input: Nil 
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// Output: int 0 if no close contact. 

// Return retVal = retVal + 2^ (sensor number) if sensor number has close 

contact 

// If all sensors have closecontact then retVal = 2+ 4+ 8+ 16 + 32 + 64 = 126 
// that detected the close in object. 

// Description: Tests irRangesAvg[] for readings that exceed the close range 
// limit minus a speed scaling value. As speed goes up, the IR range 
// 

////////////////////////////////////////////////////////////////////////////// 


int irCloseContact(void) 

{ 

int i; 
int retVal; 

retVal = FALSE; //default to no close IR contact 
for(i= 1; i < 7; i ++) 

{ //stop if any of the avg two values is close contact 


speed 


} 


if (irRangesAvg[i-1 ] >= IRSTOP_RNG - .5*v) //scale threshold with 

retVal = retVal + pow(2,i); 

} 

return retVal; //return 0 or the integer representation of the sensors 
//that have close IR contacts 


////////////////////////////////////////////////////////////////////////////// 

//9. sonar 

////////////////////////////////////////////////////////////////////////////// 

////////////////////////////////////////////////////////////////////////////// 

// Function: getSonarRanges(void) 

// Programmer: Kirk 
// Input: void 

// Output: int 0 success -2 fail for no end flag detected -1 fail no start 
flag 

// Description: Reads in the start flag then number of 
// bytes = NUM_SONAR_AZIMUTHS 

////////////////////////////////////////////////////////////////////////////// 

nodebug int getSonarRanges(void) 

{ 

int azimuthNum, i; 
char input_charlst, input_char2nd; 

int success; // good test found start of the sonar sentence 


i = 0; 

//back up check 

to prevent 

getting stuck 


// waiting in case 

sonar not operating 

while ( 

(input_charlst = 

serCgetc()) 

== -1); 

while ( 

(input_char2nd = 

serCgetc()) 

== -1); 


if( input_charlst == OxFE && input_char2nd != OxFE) 
success = TRUE; 

// while newest char and 2nd newest char 

// are not equal to start flag followed by some data value that's not OxFE 
while ( !success ) 

{ 


//eat up chars until we get a match for the start of 
//range sentence flag. Flag is OxFE 
input_charlst = input_char2nd; 


171 



while ( (input_char2nd = serCgetcO) == -1);// update the newest 

char value 

if( input_charlst == OxFE && input_char2nd != OxFE) 
success = TRUE; 

} 

input_char = input_char2nd; // we've reev'd a good data byte, so keep it 
// after we get the start of range sentence flag read in 1 byte 
// for each range value from 0 to NUM_SONAR_AZIMUTHS. After range bytes 
// get the byte that indicates which value is the newest one of the 20 
// range bytes. All other range bytes need to be shifted 
//by timeLate (ms) * - robot's velocity vector . 
for(azimuthNum = 0; azimuthNum < NUM_S0NAR_AZIMUTHS+1; azimuthNum++) 

{ 

if(azimuthNum != NUM_SONAR_AZIMUTHS) 

{ 

if(input_char == 0x02) 

sonarRanges[azimuthNum] = NOCONTACT; // default no contact range 
else //save float to array 

sonarRanges[azimuthNum]=(float) SONARCONVERSION * input_char; 


} 

else //save the integer number of the newset reported reading 

newestSonar = (int) input_char; 
while ( (input_char = serCgetcO) == -1); //get another char 


if(input_char != OxFB) 
return -2; //error no end flag 
else 

return 0; // normal got full sentence 


////////////////////////////////////////////////////////////////////////////// 

// Function: checkSonarRanges() 

// Programmer: Kirk 
// Input: Nil 

// Output: int value of the sectors around robot that have obstructions 
// Description: Function tests sonarRanges array for contacts that are closer 
// than a minimum value. 

// 

////////////////////////////////////////////////////////////////////////////// 

int checkSonarRanges(void) 

{ 


int retVal; 
int i; 

objectAhead = FALSE; //default to no object detected 

closeContact = FALSE; //default no close in contacts 
retVal = 0; 

for(i = 0; i < NUM_SONAR_AZIMUTHS ; i++) 

{ //test if object ahead of the 300 vector 
if(i >= 15 && i <= 18) 

{ 

if(sonarRanges[i] <= MIDRANGE) 

{ 

objectAhead = TRUE; 

} 

} 

if(sonarRanges[i] <= MIDRANGE ) //range = 250 means no contacts 

{ 
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retVal = retVal + pow(2,i); 

//look for close contacts either side of 300 deg axis 

if(sonarRanges[i] <= CLOSERANGE && (i >= 14 && i <=19)) 
closeContact = TRUE; 

} 

} 

return retVal; 

} 


////////////////////////////////////////////////////////////////////////////// 

//lO. Motion 

////////////////////////////////////////////////////////////////////////////// 

////////////////////////////////////////////////////////////////////////////// 
// Function: setMotorVolts 
// Programmer: Kirk Volland 

// Input: float voltage to motor 1, int direction fwd/rev for motor 1 

// float voltage to motor 2, int dir for motor 2 

// float voltage to motor 3, int dir for motor 3 

// Output: int 0 if OK transmission of motor voltages, 1 if failed 

// Description: Three float voltages are the analog voltages to output to the 

// motors. 

// 

////////////////////////////////////////////////////////////////////////////// 
int setMotorVolts(float spdl, int dirl, float spd2, int dir2, float spd3, \ 
int dir3) 

{ 


int ninl, i; 

//do a sanity check on voltages to be sure they are in acceptable range 
// less than or equal 4.0 V or greater than or equal to 0.0 
if( (spdl <= MAXVOLTS) && (spd2 <= MAXVOLTS) &&(spd3 <= MAXVOLTS) \ 

&& (spdl >= STOPVOLTS) && (spd2 >= STOPVOLTS) && (spd3 >= STOPVOLTS)) 

{ 

digOut(dirlChan, dirl); //set directions 1, 2 and 3 

digOut(dir2Chan, dir2); //set directions 1, 2 and 3 

digOut(dir3Chan, dir3); 

anaOutVolts(mtrlChan, spdl); //set voltages for 1, 2 and 3 
anaOutVolts(mtr2Chan, spd2); 

anaOutVolts(mtr3Chan, spd3); 

#if DEBUGPRTNT 

printf("setMotorVolts ml %f dl %d m2 %f d2 %d m3 %f d3 %d\n", \ 
mtrlSpd, dirl, mtr2Spd, dir2, mtr3Spd, dir3); 

#endif 

if( (spdl != STOPVOLTS II spd2 != STOPVOLTS || spd3 != STOPVOLTS ) 

&& \ 

robotNotRotating) 

robotStopped = FALSE; 
return 0; //normal exit 

} 

} 


////////////////////////////////////////////////////////////////////////////// 
// Function: stopMotorsO 
// Programmer: Kirk Volland 
// Input: nil 

// Output: int 0 if OK transmission of motor voltages, 1 if failed 
// Description: Sets global direction and motor speed variables to reverse and 
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// stop. Calls setMotorVolts(). 

// 

////////////////////////////////////////////////////////////////////////////// 
int stopMotorsO 
{ 

//dirl = REV_DIR; 
mtrlSpd = STOPVOLTS; 

//dir3 = REV_DIR; 
mtr3Spd = STOPVOLTS; 
mtr2Spd = STOPVOLTS; 

//dir2 = REV_DIR; 


V = 0; //set globals to zero 
chassisOmega = 0; 
cmdVelocity = 0; 

//set flags 
holdHeading = EALSE; 
robotNotPotating = TRUE; 

//output the voltages 

if(! setMotorVolts( mtrlSpd, dirl, mtr2Spd, dir2, mtrSSpd, dir3) ) 
return 0; 

else 

return 1; // failed to properly set voltages to all motors 

} 

////////////////////////////////////////////////////////////////////////////// 

// Eunction: spinTnPlace () 

// Programmer: Kirk Volland 
// Input: nil 

// Output: int 0 if OK transmission of motor voltages 

// retVal = retVal + 2''motrNum failed due to topping out 

// Description: Uses global chassisOmega for direction and rotational speed. 

// Sets v =0 and sets global motor speed vars to SLOWROTATESPEED. 

// Calls setMotorVolts0 . 

// 

////////////////////////////////////////////////////////////////////////////// 

int spinInPlace(void) 

{ 

float omegal, omega2, omega3; 
int retVal; 

retVal =0; // default expect normal operation, not topping 


if(chassisOmega > 0) // forward direction is counter clockwise rotation 

{ 

dirl = EWD_DIR; 
dir3 = EWD_DIR; 
dir2 = EWD_DIR; 

} 

else 

{ 

dirl = REV_DIR; 
dir3 = REV_DIR; 
dir2 = REV_DIR; 


v = 0; 

cmdVelocity = 0; 


omegal = chassisOmega; 

omegal = fabs(omegal); //output must be positive voltage, find absolute 
value 

if(omegal > MAXOMEGA) //limit output to maximum wheel rotational speed 
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omegal = MAXOMEGA; 
omega2 = omegal; 

omegaS = omegal; 

//set flags 
holdHeading = FALSE; 
robotNotRotating = FALSE; 
mtrlSpd = freqToVolts(omegal); 
if(mtrlSpd > MAXVOLTS) 

{ 

mtrlSpd = MAXVOLTS; 

retVal = 2; //set the 2''! bit for to show topping out motorl 

} 

mtr2Spd = (.99)* freqToVolts(omega2); // multiply by coeff. to account 

// for weight distribution 
if(mtr2Spd > MAXVOLTS) 

{ 

mtr2Spd = MAXVOLTS; 

retVal = retVal + 4; //set the 2^2 bit to indicate topping out 

motor2 

} 

mtrSSpd = (1.10)* freqToVolts(omegaS) ; //multiply the voltage for added 

//weight on number 3 wheel 
if(mtrSSpd > MAXVOLTS) 

{ 

mtr3Spd = MAXVOLTS; 

retVal = retVal +8; // set the 2^3 bit to indicate topping motor 

3 

} 

//output the voltages 

if(! setMotorVolts( mtrlSpd, dirl, mtr2Spd, dir2, mtrSSpd, dir3) ) 
return retVal; 

else 

return retVal; // failed to properly set voltages to all motors 


////////////////////////////////////////////////////////////////////////////// 

// Function: vector)) 

// Programmer: Kirk Volland 
// Input: nil 
// Output: int 0 if OK 

// Description: calls solveMotorSpeeds to solve for motor speeds based on 
global 

// vars V, theta and chassisOmega. chassisOmega negative is clockwise 
rotation. 

// chassisOmega in rad/s 

// 

////////////////////////////////////////////////////////////////////////////// 

int vector(void) 

{ 

solveMotorSpeeds() ; 

//set flags 
holdHeading = TRUE; 

robotStopped = FALSE; //default value false, then check 
robotNotRotating = TRUE; //default to TRUE , then check 

if(v == 0) 

robotStopped = TRUE; 

if(chassisOmega != 0 && robotStopped) // if chassisOmege is anything 
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//BUT zero, then it's rotating 

robotNotRotating = FALSE; //and robotNotRotating is FALSE 

//output the voltages 

if(! setMotorVolts( mtrlSpd, dirl, mtr2Spd, dir2, mtrSSpd, dir3) ) 

{ 

return 0; 

} 

else 

return 1; // failed to properly set voltages to all motors 

} 


////////////////////////////////////////////////////////////////////////////// 

// Function: solveMotorSpeeds(float chassisOmega) 

// Programmer: Kirk Volland 
// Input: nil 

// Output: int 0 if all calculated motorspeeds are within limits (MAXVOLTS) 

// if motor speed 1 would exceed (topping) then retVal = retVal + 2 

// if motor speed 2 would exceed (topping) then retVal = retVal t 4 

// if motor speed 3 would exceed (topping) then retVal = retVal + 8 

// Description: Uses global vars v, theta and chassisOmega. 

// Decomposes v into x and y vector components where y axis ponints to motor 1. 

// Finds each wheel's angular speed needed to produce the velocity 

// vector v in direction theta. If the calculated analog speed signal exceeds 

// MAXVOLTS, the signal is limited to MAXVOLTS. Global motor speed vars 

// mtrlSpd, mtr2Spd, mtrSSpd and global directions dirl, dir2, dir3 are set. 

////////////////////////////////////////////////////////////////////////////// 

int solveMotorSpeeds(void) 

{ 

int retVal; //return value signals if voltage is within limits 0 = 

normal 

// output byte in binary... 

// 2''3 2^2 2"'l 2''0 

// topping motor! topping motor! topping motorl 0 

norm 

char msg[256]; 
float vx, vy; 

float omegal, omega!, omega!; //wheel angular speeds in rad /s 

float veloConst; //scales omega to Hertz measured by optical tachometer 

retVal = 0; //default OK 

veloConst = 12.566; //12.566 rad/s corresponds to velocity v = 1.0 

//scales velocity v to a range zero to 

1.0 

//decompose v and theta into x and y axis components 
vx = V* veloConst * sin (theta); 
vy = V* veloConst * cos (theta); 

//do three dot products to solve for wheel speeds 
//wheel 1 

// X axis + y axis + chassis rotation angular speed 

omegal = -vx + 0 + chassisOmega; 

if(omegal < 0) 

dirl = REV_DIR; //reverse if direction is negative 

else 

dirl = FWD_DIR; 

omegal = fabs(omegal) ; //output value positive voltage, find abs. value 
if(omegal > MAXOMEGA) //limit output to maximum wheel rotational speed 
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omegal = MAXOMEGA; 


//wheel 2 

// X axis + y axis 

omega2 = vx*.5 - vy*.8660 + chassisOmega; //dot product decomposed 

into 


// x and y components 

if(omega2 < 0) 

dir2 = REV_DIR; //reverse direction 
else 

dir2 = EWD_DIR; 


omega2 = fabs(omega2) ; 

if(omega2 > MAXOMEGA) //limit output to maximum wheel rotational speed 
omega2 = MAXOMEGA; 

//wheel 3 

// X axis + y axis 

omegaS = vx*.5 + vy*.8660 + chassisOmega; 


if(omegaS < 0) 

dir3 


else 

dir3 = EWD_DIR; 


REV_DIR; 


omega3 = fabs(omega3); 

if(omega3 > MAXOMEGA) //limit output to maximum wheel rotational speed 
omega3 = MAXOMEGA; 

//solve for analog output voltage values 
//apply correction coefficients to motors 2 and 3 voltage signals 
//omegal, omega2, omega3 are the wheel speeds of each wheel in rad/s 
mtrlSpd = freqToVolts(omegal); 
if(mtrlSpd > MAXVOLTS) 

{ 

mtrlSpd = MAXVOLTS; 

retVal = 2; //set the 2''! bit for to show topping out motorl 

} 


mtr2Spd = 0.99 * freqToVolts(omega2); 
if(mtr2Spd > MAXVOLTS) 

{ 

mtr2Spd = MAXVOLTS; 

retVal = retVal + 4; //set the 2^2 bit to indicate topping out 

motor2 


} 

mtrlSpd = (1.10)* freqToVolts(omegal); 
if(mtrlSpd > MAXVOLTS) 

{ 

mtrSSpd = MAXVOLTS; 

retVal = retVal +8; // set the 2^3 bit to indicate topping motor 


} 

return retVal; 


////////////////////////////////////////////////////////////////////////////// 

// Eunction: freqToVolts(float omegain) 

// Programmer: Kirk Volland 

// Input: float omegain radians/s rotation speed wheel 
// Output: float voltsOut in Volts to apply to DAG 

// Description: Maps desired wheel angular speed to analog signal from DAC. 
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////////////////////////////////////////////////////////////////////////////// 


float freqToVolts(float omegain) 

{ 

float freq; //tachometer frequency (100 cycles per rev ) 
float voltsOut; //analog voltage for CPU to send out 
//check if speed is zero or too low 
if (omegain < .3770) 

return (0); // minimum rotation rate 6 Hz 

else //for linear response region of the tach circuit 

{ 

freq = omegaln*100/(2*PI); //tach freq 
voltsOut = (freq + 98.32) / 76.73; 

return voltsOut; 

} 

} //end funct 

////////////////////////////////////////////////////////////////////////////// 

//11. Control 

////////////////////////////////////////////////////////////////////////////// 

////////////////////////////////////////////////////////////////////////////// 
// Function: manual_control 
// Programmer: Kirk 
// Input: Nil 
// Output: Nil 

// Description: This function get the command for manual control from comms 
// module. Solves voltages needed to produce the vector direction and velocity 
// magnitude. Then sets analog voltages. 
////////////////////////////////////////////////////////////////////////////// 
void manual_control() 

{ 

char *buf;//[UDP_BUFF_SIZE]; 

char thetaStr[10], veloStr[10], omegaStr[10]; 


//strcpy(buf,GetMessageFromCommModule(&ManCtrlChan)) ; 
buf=GetMessageFromCommModule(SManCtrlChan) ; 

#if DEBUGPRINT 

printf(" manual buff chan %s\n", ManCtrlChan.Buff); 
printfC buf : %s\n", buf); 

#endif 

strcpy (thetaStr,strtok(buf," ")); //theta pass in radians 
strcpy(veloStr, strtok (NULL," ")); // velocity should be float 0 to 1.0 
strcpy(omegaStr,strtok(NULL," ")); // omega should be about 0 to 12 
#if DEBUGPRINT 

printf("theta %s, velocity %s chassis omega %s\n", \ 
thetaStr,veloStr, omegaStr); 

#endif 

theta = atof (thetaStr); 
v = atof(veloStr); 
cmdVelocity = v; 

chassisOmega = atof(omegaStr); 
if(v ==0 && chassisOmega == 0) 

stopMotors(); //do the easy thing first if just stopping 

else 

{ 

if (v ==0 ) 

spinInPlace(); // v is 0 but user commanded rotation 

else 

{ 

178 



cmdHeading = heading ; //convert course to 1 byte integer 0 to 

255 

imuHeading = 0; //reset the imu heading reference 
vector(); //move in that direction 


} 


} 


////////////////////////////////////////////////////////////////////////////// 
// Function: initRobot 
// Programmer: Kirk 
// Input: Nil 
// Output: Nil 

// Description: Sets default start values for many global vars. Sets up RS232 
// serial ports to IMU and sonar. Inits motors to stopped. Manual control. 
////////////////////////////////////////////////////////////////////////////// 
void initRobot(void) 

{ 

//Set up udp ports for communication 
InitUdpComm(); 
tcp_tick(NULL) ; 


to 

// 


sonar detector 
setup BL2600 for 


//open port with 57140 
//set to 8 bits 
//no parity 


//initialize serial port C 
serMode(0); 
ports 

serCopen (57140); 
serCdatabits (0); 
serCparity(0); 
serCwrFlush (); 
serCrdFlush(); 

#if DEBUGPRINT 

printf ( "serial C init completeXn"); 
#endif 

//set up serial port for RS232 from the IMU 
// Open serial port 
serFopen(BAUD232); 
serMode(0); 

// Clear serial buffers 
serFwrFlush(); 

serFrdFlush(); 


three RS232 
baud/s 


three-wire 


//control (motor) initialization 

dirl = REV_DIR; 

dir2 = FWD_DIR; 

dir3 = FWD_DIR; 

stopMotors(); 

manual_control_flag=l; 

#if DEBUGPRINT 

printf("motors stopped \n"); 

#endif 

cmdHeading = 0; 
imuHeading = 0; 
newimu = FALSE; 
imuZeroPoint = 510; 
heading = 0; 
headingLast = 0; 

//init the dr velocities to zero 
obsV = 0; 
obsVx = 0; 
obsVy = 0; 
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//set a default value to somewhere in North America 
//until user sends waypoint data 
waypoints[0].lat = 36.600; 
waypoints[0].Ion = 121.870; 

getOriginGPSfromGUI(); //init the nav. Origin until can get GUI input 
initNav(); 

//Navigation initialization 

current_wp_count=l; //start at waypoint 0 and move from there 

//to 1, 2, 3, etc. 

newDR= FALSE; 
compass(OxCO, 0x01); 

//error message init 
strcpY (ErrString, ""); 


////////////////////////////////////////////////////////////////////////////// 
// Function: main 
// Programmer: Kirk 
// Input: Nil 
// Output: Nil 

// Description: Sets up BL2600 I/O pins. Calls initRobot and then inits vars 
// with scope inside main. 

// Enters while loop with costates. 

////////////////////////////////////////////////////////////////////////////// 

void main(void) 

{ 

auto int i,j, k, 1, m, sendCompass, channel; 
auto int irSensorVal; 
auto float tempHeading; 

float avgVN, avgVE; 
auto int headingErr; 
auto float imuIntegralHdgErr; 

auto char val[20]; 

auto char CompassString[64];//for comms to control 
brdinit(); 

// Enable digital outputs DIO 00 and 1 for I2C with external Pullup to +K 
// Jumper J1 set to +K 

// Enable digital output DIO 02,03, 04 using +K 5V with intenal pullups 

digOutConfig(DIGOUTCONFIG) ; 

for (channel = 0; channel < 2; channel ++) 

{ 

digOut(channel, 1); // let SDA and SOL float up 

} 

i2c_init(); // initialize I2C in library function 

anaOutConfig(0, 0) ; // Analog outputs Unipolar 0 to lOV asyncrhonous 

// Analog inputs 

// Analog Configure channel pairs 0,1,2,3 for Single-Ended unipolar 
// mode of operation. 

// (Max voltage range is 0 - 20v) NOTE BL2600 can do 0 to 20 V 
anaInConfig(0, 0); 

anaInConfig(1, 0); 
anaInConfig(2, 0); 
anaInConfig(3, 0); 

anaOutPwr(1); // enable power to the DAC 

initRobot(); 

i = 0;//init to zero 
j = 0; 

irSensorVal = 0; 
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watchDogTime = MS_TIMER; 
sendCompass = FALSE; 
while (1) //do a lot 
{ 


costate 

{ 

receive packet from port(MAN_CTRL_PORT); 

if (CheckDataPresent(SManCtrlChan)==PRESENT) 

{ 

#if DEBUGPRINT 

printf("\n ManCtrlChan-> %s\n",ManCtrlChan.Buff); 

#endif 

//ClearDataPresent(SManCtrlChan) ; 

} 

waitfor(DelayMs(163)); 

} 

costate 

{ 

receive_packet_from_port(WYPNT_PORT); 
if (CheckDataPresent(SWayPtChan)==PRESENT) 

{ 

#if DEBUGPRINT 

printf("\n WayPtSock-> %s\n",WayPtChan.Buff) ; 

#endif 

// ClearDataPresent(SWayPtChan); 

} 

waitfor(DelayMs(919) ) ; 

} 

costate //waypt costate 

{ 

waitfor(CheckDataPresent(SWayPtChan)) ; 

#if DEBUGPRINT 

printf("man cont flag %d\n",manual_control_flag); 
printf("\n WayPtChan, wypt task-> %s\n",WayPtChan.Buff); 

#endif 

manual_control_flag= FALSE; 

ClearDataPresent(SManCtrlChan); 
getwaypoints(); 
initNav(); 

makeWayPts(); 
current_wp_count = 1; 

ClearDataPresent(SWayPtChan); 

V = 0.8; 

cmdVelocity = v; 

theta = rad(300); // set the vector to 060 degrees 
chassisOmega = 0; 

cmdHeading = heading ; //convert course to 1 byte int 0 to 255 
imuHeading = 0; //reset this to zero for the next leg 

vector 0; //vector robot off in the 060 degree direction 

strcat (ErrString, "Autonomous mode active\n"); 

SendToCommModule(SErrorChan, ErrString); 

SendErrorToControlStation() ; 

waitfor(DelayMs(2767)); 

} 
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//read I2C compass 

//get a compass reading with I2C bus commands 
costate 
{ 

if( compass(OxCO, 0x01) == 0) // call address OxCO 

newCompass = TRUE; // set the flag if compass data is OK 

waitfor(DelayMs(331)); // take another reading after delay 

} 

costate //alternate send compass then send GPS to GUI 

{ 

if(sendCompass) 

{ 

strcpy(CompassString, 

tempHeading = 360 * (float) heading/255; // 1 byte value to 

degrees 

sprintf(CompassString, " %f,", tempHeading); 

SendToCommModule(SCompassChan, CompassString); 
SendCompassToControlStation() ; 
sendCompass = FALSE; 

} 

else 

{ 

SendToCommModule(SGpsChan, fakeGPSsentence); 

SendGpsToControlStation() ; 

//puts(fakeGPSsentence) ; 
sendCompass = TRUE; 

} 

waitfor(DelayMs(503)); // take another reading after delay 


costate // get sonar ranges 

{ 

serCrdFlush(); //flush out any old data 
waitfor((input_char = serCgetcO) != -1); //wait until data present 

// there is serial data present on serC 
waitfor(DelayMs(59)); //Some data is present, allow time for PIC to send 

// about 2 sentences 

getSonarRanges() ; 

waitfor(DelayMs(131)); //repeat after about .157 + 59 = .2 s 
} //end get sonar data costate 

costate //update motor speeds and random walk away from obstacle 

{ 

if (! manual_control_flag ) 

{ 

checkSonarRanges() ; 

irSensorVal = irCloseContact(); 
if(closeContact I| IrSensorVal >= 32 ) 

{ 

#if DEBUGPRINT 

printf ("stopping for sonar contactin''); 

#endif 

#if DEBUGSONAR 

printf("irSensor Value %d , IR Volts: %.lf %.lf %.lf %.lf %.lf %.lf\n" ,\ 
irSensorVal,irRangesAvg[0],irRangesAvg[1] , \ 
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irRangesAvg[2], irRangesAvg[3], irRangesAvg[4],irRangesAvg[5]); 
#endif 

stopMotors(); 


waitfor(DelayMs(211)); //slow down and stop 
//reverse direction for 400 ms 

V = CRAWLSPEED; //assume reverse is clear since just came from there 

theta = rad(120); // set the vector to 120 degrees 
chassisOmega = 0; 

cmdHeading = heading ; //convert course to 1 byte integer 0 to 255 
imuHeading = 0; //reset this to zero for the next leg 

vector 0; //vector robot reverse dircection to separate from obstacle 
waitfor(DelayMs(800)); 


stopMotors0; //stop after backing up 

waitfor(DelayMs(100)); //wait for motors to stop 

spinDelay = doRandSpin(); 

waitfor(DelayMs(spinDelay) ) ; 

stopMotors(); 

waitfor(DelayMs(500)) ; //wait and check IR sensors along new route 

if (irSensorVal) //do this if it was IR detected 

{ if(irSensorVal >= 32) 

{ 

while ( (irSensorVal = irCloseContact ()) 


irSensorVal >=32 ) 


{ 

// find a random turn duration in ms 900 to 1900 ms 
spinDelay = doRandSpin(); 


&& 


waitfor(DelayMs(spinDelay)); 
stopMotors (); 

waitfor(DelayMs(1033)); // delay 1 s to so IR check path 

// and allow user to send manual commands 

} 

} 

else 

{ 

waitfor(DelayMs(1033)); //just delay 1 s and check with IR 

} 

} 

else // do this if it was sonar detected 

{ 

waitfor(DelayMs(2033)); //check the area 
checkSonarRanges (); 

while(sonarRanges[15] < CLOSERANGE || sonarRanges[16] < 

CLOSERANGE \ 

I IsonarRanges[17] < CLOSERANGE || sonarRanges[18] < CLOSERANGE ) 

// try a spin in place then wait for sonars to check area clear 

{ 

// find a random value of ms between 900 and 1900 turn duration 
spinDelay = doRandSpin(); 

waitfor(DelayMs(spinDelay) ) ; 
stopMotors() ; 

waitfor(DelayMs(2033) ) ; // delay 2s. sonars check path 

// and allow user to send manual commands 
checkSonarRanges() ; 
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} 

} //else 

//closeContact = FALSE; //kludge fix. why is it staying TRUE? 

V = CRAWLSPEED; //if clear then go again, but go slow 

theta = rad(300); // set the vector to 060 degrees 
chassisOmega = 0; 

cmdHeading = heading; //convert course to 1 byte integer 0 to 255 
imuHeading = 0; //reset this to zero for the next leg 
vector 0; //vector robot off in the 300 degree direction 
waitfor(DelayMs(600) ) ; //wait to clear obstacle 

for(l = 0; 1 < 8; 1++) 

{ 

if ( !closeContact) 

{ 

checkSonarRanges(); 

V = CRAWLSPEED; 
waitfor(DelayMs(600)) ; 


else 

stopMotors (); 
break; 


} //closecontact 


else //no close contacts. Check for medium range contacts 

{ 

if (objectAhead) // then just slow down 

{ 

V = CRAWLSPEED; //update the velocity to slow down 
vector(); 

} 

else // OK speed back up 

{ 

cmdVelocity = 0.8; 

v = cmdVelocity; 

vector (); 

} 

} //else 

}//mancontrol 
waitfor(DelayMs(131)) ; 

} //costate 


costate 


{ 


. If 


//print sonar ranges every 2 seconds 
#if DEBUGPRINT 

printf ("\nranges 0 to 09: %.lf %.lf %.lf %.lf %.lf %.lf 

\n" ,\ 


sonarRanges[0], 
sonarRanges[2], 
sonarRanges[5] , 
sonarRanges[8], 


sonarRanges[1],\ 
sonarRanges[3], sonarRanges[4] , 
sonarRanges[6], sonarRanges[7] , 
sonarRanges[9]); 


\ 


%.lf 


\ 


%. If 


% . If 
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printf("\nranges 10 to 19: %.lf %.lf %.lf %.lf %.lf %.lf %.lf %.lf %.lf %.lf 
Newest: %d\n" , \ 

sonarRanges[10 ] , \ 

sonarRanges[11], sonarRanges[12], sonarRanges[13], \ 
sonarRanges[14], sonarRanges[15], sonarRanges[16], \ 
sonarRanges[17], sonarRanges[18], sonarRanges[19], newestSonar); 

#endif 

waitfor(DelayMs(2000)) ; 


costate 

{ 

getIRVolts(); 

waitfor(DelayMs(80)); //IR sensors need 40 to 60 ms to update 


costate // get wheel speeds for dr 

{ 

wheell = 2 * wheel(OxDO, 0x01);// mult, reported speed by 2 for Hz 

waitfor(DelayMs(1)); // get wheel speed in Hz for each wheel 

wheel2= 2 * wheel(OxDO, 0x02); 

waitfor(DelayMs(1)); 

wheels = 2 * wheel(OxDO, 0x03); 

obsOmegal = wheell * 2 * PI/100; // malce wheel speed in rad/s 
if(dirl == REV_DIR) 

obsOmegal = -1 * obsOmegal; 

obsOmega2 = wheel2 * 2 * PI /lOO; //negative rotational speeds if 
if(dir2 == REV_DIR) //wheels rotating in rev. direction 

obs0mega2 = -1 * obsOmega2; 
obsOmegaS = wheels * 2 * PI /lOO; 
if(dir3 == REV_DIR) 

obsOmegaS = -1 * obsOmegaS; 

#if DEBUGDR 

printf("wheel speeds 1,2,3: %.4f %.4f %.4f \n", \ 

obsOmegal, obsOmegaS, obsOmegaS); 

#endif 

calcVeloRobotFrame(obsOmegal, obsOmegaS, obsOmegaS); 
newDR = TRUE; // flag set so dr costate will activate 

// if test the wheels and they're not spinning, robot is stopped 
if( fabs(obsOmegal) < 0.1 && fabs(obsOmegaS) < 0.1 && \ 
fabs (obsOmegaS) < 0.1) 

robotStopped = TRUE; 

else 

if (robotNotRotating) 

robotStopped = FALSE; 

waitfor(DelayMs(250)); // wheel sensor updates every 250 ms 


costate // dead reackon nav 

{ 

if(newDR) 

{ 

calcVeloEarthFrame() ; 

dr (); // dead reckon the position 

newDR = FALSE; // clear the flag 'cuz we've done 
} //the DR on existing data 
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costate //do navigation costate 

{ 

if(!manual_control_flag && !closeContact) 

{ 

navigation(current_wp_count -1, current_wp_count); 


} 

waitfor (DelayMs(2011)); //do navigation every 2 seconds 

} 

costate //fake the GPS for GUI 

{ 


makeFakeGPSpos(); 

#if DEBUGDR 

printf("\nvE %f vN %f DR Posit N %ld DR Posit E %ld\n", \ 

vE [ 0], vN[0] , DRpos.cm_N, DRpos.cm_E) ; 

#endif 

waitfor(DelayMs (919)); 

} 


costate 

{ 

waitfor( (imuChar = serFgetcO ) !=-!&& imuChar == 

waitfor(DelayMs(10)); 

if ( IgetImuO) //update if not error 

{ 

newimu = TRUE; 
if (robotStopped) 

zRotation[0] = 0; //if observe no wheel speed data, 

//robot isn't rotating 

if(fabs(zRotation[0]) > IMUROTLIMIT) //ignore rotation if too small 

{ 

imuHeading = imuHeading + zRotation[0] * \ 

((float) (imuTimes[0] - imuTimes[1])/1000 ); 


else 

{ 

zRotation[0] = 0; 

} 

serFrdFlush() ; 


} 

waitfor(DelayMs(100) ) ; 


costate //maintain heading 

{ //do this if want to maintain heading 

if( (holdHeading && (fabs(imuHeading) > IMUHEADLIMIT) \ 

&& !robotStopped && robotNotRotating ) ) 

{ 

//superimpose rotation onto existing velocity vector 
//add or subtract voltage from ALL motors' speeds 
//to spin the robot R or L to counteract any unwanted 
//chassis rotational velocity 
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//solve for integral imuHeading error 
ImuIntegralHdgErr = 0; 


value 


#if DEBUGDR 

printf("current Hdg %d Command Hdg %d\n", heading, cmdHeading) ; 
#endif 

for(m = 0; m < 6; m++) 

{ 

if (zRotation [m] < 999) //include the value if not error 


{ 

imuIntegralHdgErr = imuIntegralHdgErr + \ 

zRotation[m] * (imuTimes[m] - imuTimes[m+1]); 

} 

} 


// use IMU for heading feedback loop 

chassisOmega = - KAPPA_D * zRotation[0] - KAPPA_P * imuHeading; 

#if DEBUGDR 

printf("zRotation %.5f \tIMU Hdg %.5f\n", zRotation[0] ,\ 
imuHeading); 

#endif 

vectorO; //update the robot's motion vector 

}//end if holdHeading 
newimu = EALSE; 

waitfor(newimu); //do updates about 10 times per sec 
}//end costate 


costate //manual control costate calls manual_control() to call 
control() 

{ 

waitfor(CheckDataPresent(SManCtrlChan) ) ; 
manual_control_flag = 1; 
manual_control() ; 
waitfor(DelayMs(139)) ; 

} 

costate //watchdog for stuck motionless 

{ 

if(!manual_control_flag && v != 0 && (obsOmegal< STUCKLIMIT )\ 

&& (obsOmegal < STUCKLIMIT) && (obs0mega3 < STUCKLIMIT) ) 

{ 

//start a timer 

watchDogTime = MS_TIMER; 
waitfor(DelayMs(TIMELIM)) ; 

//recheck after TIMELIM are we still stuck? 

if(v != 0 && (obsOmegal< STUCKLIMIT ) && (obsOmega? < STUCKLIMIT) \ 

&& (obsOmegaS < STUCKLIMIT) ) 

{ 


//if still stuck then do this 
stopMotors(); 

theta = rad( deg(theta)- 180); //go reverse direction 
v = CRAWLSPEED; 


chassisOmega = 0; 

cmdHeading = heading ; //convert course to 1 byte int 0 to 255 
imuHeading = 0; //reset this to zero for the next leg 
vectorO; //vector robot off in the 300 degree direction 

187 



msDelay (300); //short duration try to back up 
stopMotors(); 

waitfor(DelayMs(100)); //wait for motors to stop 
spinDelay = doRandSpin(); 
waitfor(DelayMs(spinDelay) ) ; 
stopMotors() ; 


while( (irSensorVal = irCloseContact()) && irSensorVal >=32 


{ 

// find a random value of milliseconds between 900 to 1900 
spinDelay = doRandSpin(); 


waitfor(DelayMs(spinDelay) ) ; 
stopMotors(); 

waitfor(DelayMs(1033));// delay about Is so IR can check path 

// and allow user to send manual commands 

} 

V = CRAWLSPEED; //if clear then go again, but go slow 

theta = rad(300); // set the vector to 060 degrees 
chassisOmega = 0; 

cmdHeading = heading ; //convert course to 1 byte int. 0 to 255 
imuHeading = 0; //reset this to zero for the next leg 
vector 0; //vector robot off in the 300 degree direction 
watchDogTime = MS_TIMER; 

} 

else 

watchDogTime = MS_TIMER; //new time cuz not stuck 

} 

waitfor(DelayMs(5000)); //check every 5 sec 
}//costate 
} //while 


} / /main 
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